Frontiers in Computing and Intelligent Systems ISSN: 2832-6024 | Vol. 4, No. 3, 2023 125 A Gravity Compensation Algorithm of Robot Manipulator Control based on the Trigonometric Function Yan Wang, Xue Chen Guizhou Light Industry Technical College, Guiyang 550025, China Abstract: The conventional gravity compensation algorithm requires precise dynamic parameters and a complex matrix transformation operation, which is difficult in applications to real-time control. In this paper, a simple and practical gravity compensation algorithm is proposed based on the space geometry characteristics of a mechanical arm and the principle of torque balance. This algorithm does not require a complex calculation of space coordinate transformation and does not require obtaining all accurate dynamic models and parameters. It only requires estimating the maximum gravity moment of the mechanical arm and simply calculating the trigonometric function. Thus, this algorithm can be extended to a non-parallel shaft mechanical arm, which is suitable for N joints in space. To verify the control effect after gravity compensation, the most easily comprehensible proportional-derivative controller combined with gravity compensation is used to control two-joint and three-joint mechanical arms for simulation. With the gravity compensation and non-compensation of the mechanical arm and with a comparison with other compensation methods, such as the fixed gravity compensation algorithm, the results show that the gravity compensation algorithm can achieve better trajectory tracking control, higher steady-state precision, an effectively reduced work burden of the controller and improved system stability. Keywords: Robot Manipulator; Trajectory Tracking Control; Gravity Compensation; Trigonometric Function. 1. Introduction Service robots have a human-like appearance and structure that mainly relies on a mechanical arm to complete daily operations. These robots require trajectory tracking control to accomplish these actions and tasks. Since most of the driving force of the mechanical arm in space motion works against its own gravity, to reduce gravity disturbance and improve steady-state precision, it is necessary to compensate the gravity of the mechanical arm in the control process. At present, some progress has been made in the study of gravity compensation, mainly including a mechanical- structure method and a control-analysis method [1]. In the mechanical-structure method, some compensation devices are added to the mechanical structure in Ref. [2-5]; for example, the gravity was compensated by adding springs to each joint. In the control-analysis method, a feedforward or feedback link is added to the control strategy to compensate its own gravity. In this paper, we will study the control- analysis method. In Ref. [6, 7], a fixed position control algorithm of a mechanical arm with fixed gravity compensation was proposed, which is relatively simple and applicable to fixed position control in operating space. However, the error of the trajectory tracking control of this method is relatively large. In Ref. [8, 9], the least squares method and back-propagation neural network were applied to identify dynamic parameters such as the mass and mass centre of a mechanical arm connecting rod and to estimate the gravity term. However, this method is time-consuming and complex and is not easy to control in real time. To realise the trajectory tracking control of a mechanical arm, a gravity compensation algorithm was applied for real-time and accurate estimation of the gravity moment in Ref. [10, 11]. However, in real-life applications, it is difficult to guarantee the accuracy of gravity compensation because various accurate dynamic parameters are difficult to obtain. In Ref. [12, 13], a system dynamics model using Lagrange’s equation was built to calculate the torque generated by each part of the centre of gravity in joint space, using the torque motor to obtain the corresponding torque for gravity compensation. One disadvantage of this method is that the calculation of the gravity matrix with multiple degrees of freedom is relatively complex and the exact parameters of the Lagrange equation are not easily obtained. Based on the traditional fixed gravity compensation, the gravity compensation function of each joint was derived in Ref. [14]. A relatively accurate online gravity compensation proportional-derivative (PD) control strategy was proposed for the right arm of a de-icing robot. However, this compensation algorithm requires knowing the weight and mass centre as well as other parameters of the mechanical arm in advance and is only applicable to parallel shaft mechanical arms. It can be seen from the above review that the conventional gravity compensation algorithm requires precise dynamic parameters and a complex matrix transformation operation, which is difficult in applications in real-time control. Although some improved real-time control algorithms are simple to calculate, they are only applicable to parallel shaft mechanical arms [15]. In view of the shortcomings of the above studies, in this paper, we study a seemingly simpler gravity compensation method. This algorithm does not need complicated calculations, nor does it require obtaining all accurate dynamic models and parameters to achieve a better control effect, and it is also suitable for non-parallel shaft mechanical arms. 126 2. Gravity Compensation Algorithm of a Mechanical Arm 2.1. Analysis of Gravity Compensation of a Mechanical Arm According to the Lagrange modelling method, the mechanical arm dynamics equation with n degrees of freedom can be expressed as follows: ( ) + ( ) ( )q q q,q q q M C G Γ   (1) where ( )q t represents the angular displacement vector of a joint, which is a bounded differentiable vector; ( )qM represents the inertial matrix of the mechanical arm; ( )q,qC  represents the Coriolis force and the centrifugal force vector; ( )qG represents the gravity vector; and Γ represents the input torque. Mechanical arms usually work at a low speed. It can be seen from Eq. (1) that the dynamic performance of the robot is affected by gravity. If gravity is regarded as a disturbance, most of the work done by the input torque will be used to overcome the disturbance of the gravity moment. If gravity can be compensated effectively in the control process, the control efficiency will be greatly improved. 2.2. Calculation of the Gravity Moment of a Mechanical Arm According to the Lagrange equation for robots, the gravity term of the ith joint of the mechanical arm with n joints can be expressed as follows: 0n T kk i k k k i i g m g q     T p (2) where km is the mass of each connecting rod of the robot arm, g is the gravity acceleration vector in the basic coordinate system, 0 kT is the homogeneous transformation matrix of the k-coordinate relative to the basic coordinate system and k kp represents the distance vector from the origin of the k-coordinate to the mass centre of the connection rod k. Therefore, the gravity matrix is given as follows:  T1 2 3( ) , , ,..., nq g g g gG (3) We can apply Eq. (2) to exactly calculate the gravity term at each moment and compensate for it, that is, to make the gravity compensation term ( )q0Γ G (4) Let the input torque be ' 0Γ = Γ + Γ (5) Substituting Eqs. (3) and (4) into Eq. (1), we get ( ) + ( )q q q,q q  'M C Γ   (6) where 'Γ is the input torque after gravity compensation. Thus, the input torque can completely compensate the gravity term, which means that it can completely eliminate the disturbance of the gravity moment. However, the prerequisite is to know the dynamic parameters, such as the mass and mass centre, of each connecting rod of the robot arm. It can be seen from Eq. (2) that, to calculate the gravity moment of each joint of the mechanical arm, the two parameter values of mass km and mass centre distance ( k kp ) must be known in advance. However, it is difficult to obtain relatively accurate values using conventional measurement methods. Moreover, this method requires a large number of calculations on the rotation transformation matrix, which becomes very large when there are more joints, so it is difficult to apply this method in actual applications. 2.3. Gravity Compensation Algorithm of Parallel Shaft Mechanical Arms Perpendicular to Gravity Fig 1. Coordinate of an N-joint mechanical arm Figure 1 shows the coordinate of a mechanical arm with n joints as rotating joints, x as the connecting rod direction and z as the rotation axis direction. In the basic coordinate {0}, the gravity direction is 0-y and the rotation axis direction 0z is perpendicular to the plane 0 0x y . If not specified, the basic coordinate studied in the following section is the coordinate system shown in Fig. 1. Joint angle 1q is defined as the angle between 0x and the connecting rod 1x in the basic coordinate system and nq is defined as the angle between the connection rods -1nx and nx . According to the definition of joint angle and the property of gravity, when the mechanical arm is fully expanded along the direction perpendicular to gravity, in other words, when 1 2 n 0q q q  …… , the maximum gravity moment is received. If the mechanical arm meets the following two conditions, then the maximum value of the gravity moment at each joint can be derived from Eq. (2): 1) All joints are rotating joints. 2) For all connecting rods, the mass centre of the i connecting rod is on the line between the i coordinate system and the origin of the i +1 coordinate system. 1 1 1 2 1 2 2 22 3 3 3 3 3 4 1 1 1 1 ... ... n c k k n c k k n c k k n c n n n n n c n n m l l m g m l l mg g gm l l m g g m l l m m l                                                           (7) where ng represents the maximum value of the gravity moment of the nth joint, nm is the gravity of the nth x0 y0 z0 q1 θ1 xn-1 yn-1 zn-1 qn θn x1 y1 z1 xn yn zn z0 127 connecting rod, nl is the length of the nth connecting rod, c nl is the distance between the mass centre of the nth connecting rod and the origin of the nth coordinate system and g is the acceleration due to gravity. Parallel shaft mechanical arm perpendicular to gravity refers to all joint axis angles n k  (k is an integer); in other words, all joint axes of the mechanical arm are perpendicular to the direction of gravity and parallel to each other. According to the property of torque and the space geometry characteristics of the mechanical arm, we consider the maximum gravity moment and joint angle of each joint as variables. Thus, the online gravity compensation algorithm is designed as follows: 1 1 1 1 2 1 2 2 2 1 2 3 2 3 3 3 1 2 33 4 11 1 1 1 1 1 ( ) cos( ) ( ) ( ) cos( + ) ( ) ( ) cos ( + + )( ) ... ... ( ) ( ) cos( ) ( ) cos( ) n c k k n c k k n c k k nn c n n n n i n i n c n n i i m l l m g q G q m l l m g q q G q m l l m g q q qG q G q m l l m g q G q m l g q                                                                 (8) where ( )nG q represents the torque to be compensated for the nth joint, nm is the gravity of the nth connecting rod, nl is the length of the nth connecting rod, c nl is the distance between the mass centre of the nth connecting rod and the origin of the nth coordinate system, g is the acceleration due to gravity and iq is the ith joint angle. 2.4. General Gravity Compensation Algorithm of a Space Manipulator Fig 2. Definition of the joint axis angle As shown in Fig. 2, the joint axis angle n is defined as the angle between the rotation joint axis nz and the horizontal plane 0 0z x . Whether and how much the space manipulator requires gravity compensation during the control process are closely related to the joint axis angle. As long as the joint axis angle θ / 2n k (k is a non-zero integer), the manipulator will be disturbed by the gravity and it will be necessary to compensate the gravity. According to the property of torque and the space geometry characteristics of the mechanical arm, taking the maximum gravity moment, joint axis angle and joint angle of each joint as variables, the online gravity compensation algorithm is designed as follows: 1 1 1 1 1 2 1 2 2 2 2 1 1 2 3 2 3 3 3 3 1 1 2 2 33 4 21 1 1 1 1 1 1 ( ) cos( ) ( ) ( ) cos( + ) ( ) ( ) cos( + + )( ) ... ... ( ) ( ) cos( + ) ( ) n c k k n c k k n c k k nn c n n n n n i i n n i n n m l l m g q G q m l l m g q q G q m l l m g q q qG q G q m l l m g q q G q m l                                              1 1 cos( ) n c n i i n i g q q                                      (9) where ( )nG q represents the torque to be compensated for the nth joint, nm is the gravity of the nth connecting rod, nl is the length of the nth connecting rod, c nl is the distance between the mass centre of the nth connecting rod and the origin of the nth coordinate system, g is the acceleration due to gravity, iq is the ith joint angle and n is the compensation coefficient. The general gravity compensation algorithm of a space manipulator [see Eq. (9)] has a compensation coefficient n compared with the gravity compensation algorithm of parallel shaft mechanical arms [see Eq. (8)]. This coefficient is related to the joint axis angle, which is defined as follows: 1 1 2 2 3 3 1 1 cos ( ) cos ( ) cos ( ) cos ( ) cos ( ) n n n n                                                    … …… (10) The compensation coefficient is zero when the th joint axis angle / 2n k  (k is a non-zero integer) because, at this point, the joint axis is parallel to the direction of gravity and gravity has a little influence on the rotation motion of the joint. Therefore, no gravity compensation is required. Most of the previous studies have been conducted on parallel shaft mechanical arms with all joint axis angles of n k  (k is an integer). In this case, all joint axes are perpendicular to the direction of gravity and the compensation coefficient is 1. The rotation motion of the joint is mostly affected by gravity. When the direction of the joint axis is neither parallel nor perpendicular to the direction of gravity, the compensation coefficient n of each joint is used to indicate the influence of gravity on the rotation motion of the joint. The larger the value is, the larger the disturbance of gravity is, and the corresponding gravity compensation is calculated according to the algorithm. Therefore, the gravity compensation algorithm modified by the compensation coefficient is suitable for non-parallel shaft mechanical arms of any joint axis angle, which extends the application scope of the algorithm in this paper. 128 3. Control Scheme Design and Stability Analysis 3.1. Control Scheme Design To verify the feasibility and effectiveness of the compensation algorithm of the mechanical arm trajectory tracking control system, the simplest PD control is adopted for the convenience of analysis. The control process of the dual-joint mechanical arm system is shown as follows. PD Controller Joint Driver qd q Gravity Compensation — e Fig 3. Control flow chart The expected input of the control system is dq , the output variable is the real-time joint angle q and the error variable is defined as de = q q , dq q e   (11) Among these, the PD controller uses input and output deviation for control and its control law is given as ( )P v d= q Γ K e K e G (12) where PK and vK are n-dimensional positive definite diagonal constant matrices, and the variables G  are defined in Eq. (9).  T1 2 n( )= ( ), ( ) ( )q G q G q G qG ,..., (13) Using Eqs. (1) and (12), we derive ( ) + ( ) ( ) ( )P v dq q,q q q   M q C q G K e K e G   (14) 3.2. System Stability Analysis The closed-loop control equation [see Eq. (14)] and Eq. (11) are rewritten in the state space form as follows: 1( ) + ( ) + ( ) ( )p v d qd q e q q,q q q qdt                e M K K C G Gq      (15) The Lyapunov function is constructed as follows: ( ) ( ) ( )V e,q P e h e Tq q  (16) Where T T T 1 1 ( )= ( ) 2 1 1 ( ) ( ) ( ) ( ) ( ) 2 2 d d u p d d p d P e q -e h e u q e k q q q            M e K e e G G K G (17) Where ( )u q represents the potential energy function of the mechanical arm system.  ( )minu q k u q (18) Assuming the robot manipulator joints to be all rotary, then ( ) 0uu q k  (19) It can be proved that the Lyapunov function is non-negative definite. Take the derivative of this function with respect to time: T T T T T1 ( , ) ( ) ( ) ( ) ( ) 2 P dV e q q q q q    q M q q M q q G e K e e G        (20) According to the definition of the gravity matrix, ( ) ( )q q q    G u (21) Using Eq. (15), we can derive T T T T T( , ) ( ) ( )P v d P dV e q q q    q K e q K q q G e K e e G       (22) Using the properties of the mechanical arm kinetic equation [16], we obtain T 1 ( ) ( ) 0 2 q q,q     q M C q  (23) According to formula (11), we can derive e = q  (24) Making Eqs. (22), (23) and (24) simultaneously, we can derive T( , ) 0vV e q     q K q (25) We know from Eq. (25) that when vK is positive definite， ( , )V e q  is negative semi-definite. According to the Lyapunov stability criterion, the position error e and velocity ( )q t are bounded under any initial condition. According to La Salle’s theorem [15], [ , ] [0,0]e q  is the global asymptotically stable equilibrium point of the system, which proves the stability of the system and meets the basic requirements of the control system. 4. Simulation and Research of Mechanical Arm Gravity Compensation 4.1. Dual-Joint Mechanical Arm Simulation To verify the effectiveness of the gravity compensation control method, an experimental simulation test was conducted on a dual-joint parallel shaft mechanical arm, shown in Fig. 3. The end motion track of the mechanical arm is designed as a circle, shown in Fig. 4. m1 m2 G x0 y0 R 50mm x1 y1 x2 y2 l1 l2 q1 q2 Fig 4. Configuration and motion track of the dual-joint mechanical arm 129 The ADAMS software was used to model the dual-joint mechanical arm, and MATLAB Simulink is used to design the PD gravity compensation controller. The simulation test scheme is as follows. 1) Setting up the dual-joint parallel shaft mechanical arm model as shown in Fig. 4, the parameters are shown as Table 1. Table 1. Parameters of the dual-joint mechanical arm Parameter/ (unit) 1 2, /(mm)l l 1 1, /(mm)c cl l 1 2, /(kg)m m Value 250 125 0.88 2) To make the end motion track of the mechanical arm follow the specified trajectory, it is necessary to calculate the joint variables of the two joints through the trajectory equation and then use these two-joint variables as the input of the control system model. Take 1q and 2q as the angular displacement of joint 1 and joint 2, respectively, with 1l and 2l being the respective lengths of the two joints. The non- linear equations are 1 1 2 2 1 1 2 2 *cos( ) *cos( ) *sin( ) *sin( ) x l q l q y l q l q      (26) The time interval t is set as 0–6.28 s with 400 points in the middle, and the joint angles at each time point are solved to obtain the joint space trajectory over the entire time interval. 3) Using the algorithm shown in Eq. (9) and the control process shown in Fig. 2, gravity compensation is carried out using the joint angle from the feedback, and the gravity compensation algorithm of the dual-joint parallel shaft mechanical arm is obtained as follows: 1 1 1 2 1 2 2 1 2 2 2 1 2 =[( ) ]cos( ) = cos( + ) c c c G q m l m l g m l g q G q m l g q q      （ ） （ ） (27) Fig 5. Trajectory tracking curve without weight compensation The parameters of the PD controller are selected as v 40 0 = 0 20p        K K . First, we look at the simulation results without gravity compensation. As can be seen from Fig. 5, the PD controller only uses deviations to carry out the test. If the control does not conduct gravity compensation, the disturbance of gravity will not be able to be eliminated, so the error will be relatively large and it will be difficult to conduct trajectory tracking control. To verify the performance of the gravity compensation algorithm in this paper, the fixed gravity compensation algorithm proposed in [6, 7] was adopted for comparison. Fig 6. Trajectory tracking curve with weight compensation. Fig 7. Tracking error curve of the compensation algorithm in this paper T h e a n g u la r d is p la ce m e n t o f j o in t o n e (d e g re e ) T h e a n gu la r d is p la ce m e n t o f j o in t t w o (d e gr e e ) T he a ng ul ar d is pl ac em en t of jo in t o ne (d eg re e) T he a ng ul ar d is pl ac em en t of jo in t t w o( de gr ee ) er ro r( de gr ee ) 130 Fig 8. Tracking error curve of a fixed compensation algorithm It can be seen from Fig. 6 that the trajectory tracking control can be well realized after the adoption of gravity compensation. According to Fig. 7, the gravity compensation algorithm adopted in this paper can cause the tracking error 1.5oe  and steady-state precision to be high. In Fig. 8, the tracking error of the fixed compensation algorithm exceeds 3o . Therefore, the gravity compensation algorithm in this paper is better in terms of error control. Next, an input sinusoidal signal is used to test the dynamic tracking performance of the system. It is assumed that the input signal of joints 1 and 2 is 1 2 70sin( ) 70sin( ) t t      (28) The tracking effect of the dual-joint mechanical arm under different sinusoidal signals is as follows. Fig 9. Trajectory tracking curve under a sinusoidal signal It can be seen from Fig. 9 that the gravity compensation algorithm has a better tracking performance for general sinusoidal waves and the tracking error is 2oe  , thus achieving better trajectory tracking control. 4.2. Three-joint Mechanical Arm Simulation To further verify the effectiveness of the gravity compensation control algorithm, a gravity compensation simulation was performed on the three-joint mechanical arm shown in Fig. 10. The gravity parameters are shown in Table 2. Fig 10. Structure and model of the three-joint mechanical arm Table 2. Parameters of the three-joint mechanical arm Parameter Rod length/(mm) Mass/(kg) L1 L2 L3 M1 M1 M3 Value 100 125 125 1 0.88 0.88 Since the terminal attitude is not considered, only the first three joints controlling the terminal position are used for the trajectory tracking control experiment. The trajectory motion curves of the three-joint angles are shown in the following: 1 2 3 /3sin(0.5 ) /3sin(0.5 ) /3sin(0.5 ) 0.3556 t t t                (29) In addition, the gravity compensation algorithm based on the trigonometric function shown in Eq. (12) is utilised, and the parameters are set as follows: 1000 500 500 p          K 10 100 100 v          K 2 2 3 0 94cos( ) 18cos( ) G q q q          According to Eq. (10), since the rotation axis of the first joint is parallel to the direction of gravity, its gravity compensation coefficients are set as 1 1=0 =0G  ; thus, no gravity compensation is required. The experimental results are shown as follows. Figures 11 and 12 represent the angular displacement and tracking error curves of each joint, er ro r( de gr ee ) T h e a n gu la r d is p la ce m e n t o f j o in t o n e (d e gr e e ) T h e a n gu la r d is p la ce m e n t o f j o in t t w o (d e gr e e ) 131 respectively. Fig 11. Trajectory tracking curve of the three-joint mechanical arm As can be seen from Figs. 11 and 12, the error of the gravity compensation algorithm in this paper is no more than 0.045 rad, whereas the error of the fixed compensation algorithm is more than 0.05 rad. Therefore, the compensation algorithm in this paper has a better compensation effect and can realise better trajectory tracking control. 5. Conclusion In this paper, a gravity compensation method with an easy algorithm and implementation was proposed. Furthermore, the concept of a ‘compensation coefficient’ was introduced and applied to gravity compensation based on the space geometry characteristics of a mechanical arm and the principle of torque balance. (a) The algorithm in this paper (b) Fixed compensation algorithm Fig 12. Tracking error curve of the three-joint mechanical arm. This algorithm does not require calculating the gravity compensation through complex matrix space transformation; it only requires knowing the mass parameters and it can compensate the gravity online through simple trigonometric function transformation, which can be extended and applied to multi-joint non-parallel shaft mechanical arms. Through simulation, experimental research and analysis, this compensation algorithm is simple to apply and can compensate the gravity well in the trajectory tracking control of a robotic manipulator. Acknowledgments Sponsored by the "2022 Doctoral Research Start-up Funding Project of Guizhou Polytechnic of Light Industry" (NO. 22QYBS10). References [1] F.A. Machorro-Fernandez, V. Parra-Vega, E. Olguin-Diaz. Active Mechanical Compensation to Obtain Gravity-Free Robots: Modeling, Control, Design and Preliminary Experimental Results[J]. Ieee Int Conf Robot, 2009: 4346-4351. [2] C. Cho, S. Kang. Design of a Static Balancing Mechanism with Unit Gravity Compensators[J]. Ieee Int C Int Robot, 2011. [3] A. Mashayekhi, A. Nahvi, M. Yazdani, M. Mohammadi Moghadam, M. Arbabtafti, M. Norouzi. VirSense: a novel haptic device with fixed-base motors and a gravity compensation system[J]. Industrial Robot: An International Journal, 2014, 41(1): 37-49. er ro r( ra d) er ro r( ra d) 132 [4] S.-H. Kim, C.-H. Cho. Incomplete gravity compensator f or a 4-DOF manipulator[J]. Journal of Mechanical Science and Technology, 2015, 29(10): 4417-4426. [5] C. Cho, S. Kang. Design of a Static Balancing Mechanism for a Serial Manipulator with an Unconstrained Joint Space Using One-DOF Gravity Compensators[J]. Ieee T Robot, 2014, 30(2): 421-431. [6] YANG Y G, PENG Z, WANG T, WANG Y F. Application of fuzzy immune PD controller with fixed gravity compensation to displacement control for manipulator actuated by pneumatic muscle actuator[J]. Robot, 2008, (03): 259-263. [7] WANG Z Z, WANG W L, JIN C S, SUN S H. Research of Robot Controlling with Fixed Gravity Compensation[J]. Machinery Design and Manufacture, 2015, (03): 166-169. [8] XIE G H, ZHANG J C. Experimental Research on Gravity Compensation for Humanoid Robot Arm[J]. Journal of Mechanical Transmission, 2010, (07): 26-28. [9] XIE G H, JIN M N, ZHANG J C, WU D M. Research on Gravity Compensation Using BP- NN Algorithm for Robot Arm[J]. Process Automation Instrumentation, 2012, (02): 22- 24. [10] ZHAO J, REN S J, YU Z Y. Study of Manipulator PD Control System Based on Gravity Compensation[J]. Modern Scientific Instruments, 2012, (05): 58-59. [11] Y.C. Liu, N. Chopra. Gravity-Compensation-Driven Position Regulation for Robotic Systems Under Input/Output Delays [J]. IEEE Transactions on Control Systems Technology, 2014, 22(3): 995-1005. [12] WANG H M, DU Z J, YAN Z Y, LIU R Q. Gravity Compensation Algorithm for Hybrid Master Manipulator[J]. Robot, 2014, (01): 111-116+128. [13] E.-J. Kim, K. Seki, M. Iwasaki. Motion control of industrial robots by considering serial two-link robot arm model with joint nonlinearities[J]. Journal of Mechanical Science and Technology, 2014, 28(4): 1519-1527. [14] CHEN Y J, WANG Y N. Tracking Control of Deicing Robot on Voltage Transmission Lines Based on Online Gravity Compensation[J]. Information and Control, 2013, (04): 401- 406. [15] Emmerich C, Nordmann A, Swadzba A, et al. Assisted Gravity Compensation to cope with the complexity of kinesthetic teaching on redundant robots[C]// IEEE International Conference on Robotics and Automation. IEEE, 2013:4322- 4328. [16] R. Kelly, V. Santibáñez, A. Loría. Control of robot manipulators in joint space[M]. London: Springer, 2005.