6th International Conferene on Advaned Design and Manufaturing Engineering (ICADME 2016) Analysis and verifiation of multi-rotors attitude ontrol algorithms in Pixhawk Fangzhen Lin 1, a 1 Beihang University, China Key words.pixhawk, attitude ontrol, angle error, asade PID Astrat. Pixhawk is an open-soure flight ontrol system. The researh on the attitude ontrol algorithm Pixhawk uses provides a good referene for the use and development of the Pixhawk. In this paper, it provides a theoretial analysis of the attitude ontrol algorithm of multi-rotors in Pixhawk, and it also arried out simulations and flight experiments to verify the performane of the multi-rotor attitude ontrol algorithm. It has shown that the attitude angle error algorithm ased on rotation vetor and asade PID ontroller the Pixhawk uses not only an stailize the multi-rotor around hovering, ut also an realize omplex maneuvers. introdution in reent years, the UAV is inreasingly popular, espeially small sale UAV, represented y the multi-rotor. Flight ontrol systems as the ore of UAV also attrat more and more attention, at the same time, open soure flight ontrol systems have reeived onsiderale attention from researh ommunities. with reliale hardware and exellent software algorithm, Pixhawk is most popular open soure flight ontrol system, while the researh on the ontrol algorithm in Pixhawk is poor, in this paper, the attitude ontrol algorithm of multi-rotors is onerned, we mainly talk aout the method of alulating the angle error and attitude ontroller, at the same time, we ondut simulation experiments and flight experiments to verify the attitude ontrol algorithm. The attitude ontrol algorithm of multi-rotors The attitude angle error algorithm In Pixhawk, angle errors are alulated ased on rotation vetor while angle errors are sutration etween desired attitude angle and urrent attitude angles onventionally[1-3]. In this paper, we denote roll-pith-yaw angles of the multi-rotor as,, and denote target roll-pith-yaw angles as,we represent their orresponding diretion osine matrix using Lg and Lg,we also define the orresponding ody frame using Ox y z and Ox y z. as we know,the third row of the Lg is the projetion of oz -axis in NED oordinate system, denoted as z _ g, similarly the third row of the Lg is the projetion of oz -axis in NED oordinate system, denoted as z _ g.in addition, z _ g and z _ g are unit vetor, the dot produt of z _ g and z _ g is the osine of 2016. The authors - Pulished y Atlantis Press 612
the angle etween z _ g and z _ g, due to the monotoni of the osine funtion etween we an use the angle etween, so we an use z _ g z _ g to measure the deviation etween the urrent attitude and target attitude. We use the different method to alulate the angle error etween the urrent attitude and target attitude aording to the value of z _ g z _ g. if z _ g z _ g >0, we onsider that the deviation etween the urrent attitude and target attitude is small, angle errors are alulated y the following method: 1) Rotating the multi-rotor around the axis of rotation to make the oz oz -axis, as the figure 1 shows. -axis oinide with Figure 1 We denoted the axis of rotation and the angle of rotation as e _ R _ z _ axis and e _ R _ z _ angle. e _ R _ z _ axis is the axis of rotation expressed in urrent ody frame Ox y z,and the e _ R is the vetor whih onsists of rotation angles around the axis of Ox y z in that proess. As the formula (1), (2), (3) shows lew. e _ R _ z _ axis l, m, n L T T g z _ g z _ g z _ g z _ g. (1) z z e _ R _ z _ angle artan( ). (2) z z e _ R e _ R _ z _ axis e _ R _ z _ angle. (3) After the rotation aove, the multi-rotor reah a transition attitude, the orresponding ody frame and diretion osine matrix are denoted as (5) shows lew. g 2 Ox y z and L g. As the formula (4), L L (IU sin (1 os ) U ). (4) g 613
0 n m U n 0 l. (5) m l 0 2) Rotating the multi-rotor around oz -axis to make the Ox y z oinide with the target ody frame Ox y z, as figure 2 shows. Figure 2 The rotation angle is denoted as yaw _ err.the first row of diretion osine matrix Lg is ox -axis expressed in NED oordinate system, denoted as x _ g, similarly, the first row of diretion osine matrix Lg is ox -axis expressed in NED oordinate system, denoted as x _ g, x _ g and x _ g are unit vetors we an get the yaw _ err from them. As the formula (6) shows lew. ( x _ g x _ g) z _ g yaw _ err artan. (6) x _ g x _ g 3) Calulate the angle error During the flight, pilots mainly adjust the roll angle and pith angle, so in the first rotation proess, e _ R(3) is smaller than e _ R(1) and e _ R(2) that an e ignored. Pith and roll ontroller is mainly doing traking ontroller. In the seond rotation proess, yaw ontroller is mainly doing traking ontroller. As we know, the response of the motion of pith and roll is faster than yaw. The deomposition deouples the multi-rotor to sequentially exeute fast response motion and slow response motion. So in the whole proess, to make urrent the ody frame oinide with target ody frame, the rotation angle around Ox -axis is e _ R (1), the rotation angle around Oy -axis is e _ R (2), to eliminate the influene of the slow response of yaw motion, the rotation angle around Oz -axis an e 2 2 yaw _ err *os os, if the target attitude is near hovering, the 2 2 yaw _ err *os os is igger that make it easier to return hovering for multi-rotors. Overall, if z _ g z _ g >0, angle errors of roll-pith-yaw angles are e _ R (1) e _ R (2) 2 2 yaw _ err *os os. 614
if z _ g z _ g <0, we onsider that the deviation etween the urrent attitude and target attitude is large, angle errors are alaulated y the following method: 1) Rotating the multi-rotor around the axis of rotation to make the Oxyz Ox y z, as figure 3 shows. oinide with Figure 3 The diretion osine matrix of rotation proess an e represented y C,, C, an e expressed with a group of quaternion, the quaternion is denoted as q q, q, q, q 0 1 2 3. We also denote the rotation-axis as e _ R _ d and the vetor of rotation angles as e _ R _ d _ angle. As the formula (7), (8), (9) shows lew. T g 2 2 2 2 2 2 1 2 0 3 0 1 2 3 2 3 0 1 q q q q q q q q q q q q 2 2 2 2 q0 q1 q2 q3 q1q 2 q0q3 q0q2 q1q 3 2 2. 2 2 2 2 2 0 2 1 3 2 2 3 0 1 0 1 2 3 C ' L L q q q q q q q q q q q q e _ R _ d g q, q, q 1 2 3 2 2 2 1 2 3 q q q e _ R _ d _ angle e _ R _ d*artan. (8) q 0 2 2 2 1 2 3 q q q (7). (9) 2) Calulate the angle error In the whole proess, to make urrent the ody frame oinide with target ody frame, the rotation angle around Ox -axis, Oy -axis, Oz -axis are e _ R _ d _ angle (1), e _ R _ d _ angle (2), e _ R _ d _ angle (3). In order to get ontinuous angle errors, the final angle error an e expressed with a weighted average of angle errors got when deviation is large and small. The final angle error is denoted as error. As the formula (10), (11) shows lew. 2 2 2 diret _ w ( z _ g z _ g) *os os. (10) 2 2 error ={ e_r(1), e_r(2), yaw _ err *os os }(1- diret_w) + e_r_d diret_w. (11) 615
diret _ w will get igger when the angle deviation is igger, so the weight of angle errors got when angle deviation is igger. In addition, if the target attitude around hovering, the angle error alulated is igger, so feedak is igger, multi-rotor an return hovering more quikly. At the same time, with the angle error alulated y rotation vetor, the path of attitude regulation is shorter than onventional algorithms[4-7]. Casade PID attitude ontroller In Pixhawk, the attitude ontroller is the asade PID ontroller, the inner loop is to ontrol the attitude angular rate whih responds quikly, the outer loop is to ontrol the attitude angle whih responds slow. As figure 4 shows. Figure 4 Firstly, getting the angle error using the method shown aove when the urrent attitude and target attitude is known, seondly, getting the target angular rate from the outer PID ontroller, thirdly, target angular rates sutrat urrent angular rates to get angular rates. Lastly, getting the final output from the inner PID ontroller. Overall, Multi-rotors an follow the target attitude angular rate, and thus an e ale to follow the target attitude angle[8-10]. Verifiation of the effet of the ontroller In order to verify the effet of the ontrol algorithm, we estalish flight ontrol system mathematial model of multi-rotors and ondut simulation experiments, on the other hand, we ondut flight experiments using a four-rotor airraft whih uses the ontrol algorithm show aove. The simulation experiment The performane of the ontrol algorithm an e ompared y step response. In simulation experiments, several typial ases are validated[11]. 1) Multi-rotor is hovering initially, a step response is generated to simulate a ontrol ommand that make pith angle and roll angle reah 5 degrees. As figure 5 shows 6 5 phi theta roll angle,pith angle/ 4 3 2 1 0 0 1 2 3 4 5 6 7 8 9 10 time/s Figure 5 2) Multi-rotor is hovering initially, a step response is generated to simulate a ontrol ommand that make pith angle and roll angle reah 50 degrees. As figure 6 shows 616
60 50 phi theta roll angle,pith angle/ 40 30 20 10 0 0 1 2 3 4 5 6 7 8 9 10 time/s Figure 6 Flight experiment We also arry out flight experiments outdoor. Figure 7 shows the ontrol performane during flight. The error etween the target attitude and urrent attitude is less than 3 onventionlly, even doing large angle movement, it an also trak the target attitude quikly and preisely. As figure 7 shows Figure 7 From the result shown aove, we an know that multi-rotor an ontrol the attitude quikly and preisely with the ontrol algorithm. Conlusion In this paper, we present the angle errors whih alulated ased on rotation vetor and asade PID ontroller ased on the attitude angle and the angular rate. With the algorithm, multi-rotors an make the path shorter and smoother when hanging the attitude, and trak the target attitude quikly and preisely whih is important for flight ontrol systems. Referenes [1]. Jiaji He, Modeling the Quad-rotor and Control Strategy researh[d]. Northeastern University. 2012. In Chinese. [2]. Guanghao Tan, Design and Implementation of Attitude Control System for a Quadrotor UAV. Dalian University of Tehnology. 2013. In Chinese. [3]. Lei Zhang, Hao Li, Attitude Control of Four- Rotor Airraft via Fuzzy PID. Computer Simulation, 2014(08): p.73-77. In Chinese. [4].Yun, Y., et al. High performane full attitude ontrol of a quadrotor on SO(3). in IEEE International Conferene on Rootis and Automation. 2015. [5].Mellinger, D. and V. Kumar, Minimum snap trajetory generation and ontrol for quadrotors. 2011 IEEE International Conferene on Rootis and Automation.2011. [6]. Mellinger, Daniel Warren, Trajetory Generation and Control for Quadrotors. Pulily Aessile Penn Dissertations.2012, p.524-547. 617
[7].MKerrow, P.,Modelling the Draganflyer four-rotor heliopter. IEEE International Conferene on Rootis &. 2004. [8]Qingrui Feng, Hailong Pei, The appliation of asade PID ontrol in UAV attitude ontrol. Miroomputer Information, 2009. 25(22): p.9-10. In Chinese. [9].Chengfu Wu, Xiaoqi Liu, Xu Yuan, Modeling and PID ontrol for a quadrotor. Eletroni Design Engineering, 2012(16): p.68-70. In Chinese. [10]. Xiaoo Li, Shuxi Song, Hovering Control for Quadrotor Unmanned Heliopter Based on Fuzzy Self-tuning PID Algorithm. Control Engineering of China, 2013(05): p.910-914. In Chinese. [11]. Shaoin Ding, Modeling and Quaternion Control of X-Type Quadrotor. Journal of System Simulation, 2015(12): p.57-62. In Chinese. 618