1. INVERSE KINEMATICS ANALYSIS TRAJECTORY PLANNING FOR A ROBOT ARM Proceedings of 2011 8th Asian Control Conference Kaohsiung, Taiwan, May 15-18, 2011 Guo-Shing Huang, Chiou-Kou Tung, Hsiung-Cheng Lin, and Shun-Hui Hsiao Department of Electronic Engineering, National Chin-Yi University of Technology Taichung 41101, Taiwan, ROC Adviser ： Ming-Yuan Shieh Student ID ： M9920105 Student ： Chun-Ming Su 1

2. OUTLINE Abstract Introduction Structure of the 6-DOF humanoid robot arm A. Hardware description B. Structure of the 6-DOF humanoid robot arm Kinematics of the humanoid robot arm A. Forward kinematics B. Inverse kinematics Simulation results Conclusion 2

3. ABSTRACT This paper presents a 6-DOF robot arm system. Using the robot arm assembled by seven AI servos (RX-64), set up robot‘s coordinate system with the D-H notation method. To adjust and drive the robot arm to the coordinates of folder and place between ones and the target object, make the angle of the shaft position can accurately locate the direction for all axes of the robot arm and obtain the optimal motion path. 3

4. Under the experimental test, we position the object using the camera which was installed on robot arm, according to the attitude of the object, control the robot arm through the analysis result of inverse kinematics equation in order to make the robot arm achieve the action of exact grasping object. Finally, the robot arm system will be used on the meal service robot to serve for guests. 4

5. INTRODUCTION The robot arm system is widely used in production, processing, product transportation, domestic services and other fields. But the robot arm which is fixed on the platform, its work space is very limited. In addition, the motion path of a pairs of moving robot arm makes the analysis more complex using inverse kinematics, all these questions are concerned by a lot of experts of the robot field. 5

6. This paper proposes a method to solve the inverse kinematics for the robot arm which makes up with six-degree-of-freedom joints in order to analyze the inverse kinematics of the robot arm, can find the end-effector position and posture of the robot arm that can meet the qualification in the workspace, and then realize the operation to reach the target. The experimental results verify that the proposed inverse kinematics analysis approach in this paper has the higher accuracy. 6

7. STRUCTURE OF THE 6-DOF HUMANOID ROBOT ARM This section describes in more detail the main hardware structure and system structure that were adopted in this work. A. Hardware description B. Structure of the 6-DOF humanoid robot arm 7

8. A. HARDWARE DESCRIPTION In this paper, the embedded computer is as the main controller, adopt Dynamixel RX-64 servo motor which is produced by ROBOTIS Company as the main motive force of the robot arm, as shown in Fig. 1. (a)(b) Figure 1. (a) Dynamixel RX-64 servo motor. (b) Robot arm. 8

9. The system structure is shown in Fig. 2. 9

10. B. STRUCTURE OF THE 6-DOF HUMANOID ROBOT ARM The robot arm comprises shoulder joint, upper arm, elbow joint, lower arm, wrist and the end- effecter. The robot arm has six degrees of freedom, three degrees of freedom are located at the shoulder, the elbow has two degrees of freedom, and one degree of freedom is assigned on the wrist. 10

11. Fig. 3 shows the structure diagram of the right robot arm. 11

12. KINEMATICS OF THE HUMANOID ROBOT ARM A D-H coordinate system of the humanoid robot arm is found in Fig 4. The parameters of link are shown in Table I. 12

13. 13

14. According to D-H coordinate system, the homogeneous trans-formation matrix i T (i = 1,2,...,6) expresses 14

15. A. FORWARD KINEMATICS Institute the link parameters from Table I into (1), the pose of end-effector can be obtained 15

16. B. INVERSE KINEMATICS In finding out solution of the inverse kinematics, using the obtained homogeneous transform matrix of robot arm, to the formula of homogeneous transform matrix of the fix target coordinate in (2), multiples inverse matrix in (3), the inverse kinematics is derived by using to compare with every element of two matrices. 16

17. Let elements (1, 4), (2, 4), (3, 4) of both sides in (3) be equal respectively, add the above three equations after them to be squared. where 17

18. Since ∈ (−180°~ −70°), we have where Atan 2( ⋅ ) is the four quadrant arctangent function. According to the orthogonality of rotation matrix, elements of homogenous matrix described in (2) satisfy following equations: 18

19. In (2), we get Let elements (1, 4), (2, 4), (3, 4) of both sides in (8) be equal respectively, we have 19

20. Add the above three equations after them to be squared, and follow up (7), we obtain the solution of where 20

21. In (9), let Since ∈ (−180°~ −70°), we obtain the solution of, 21

22. Then the solution of is Where sign( ⋅ ) is the sign function, it is defined as We rearrange (2) and get 22

23. Let element of both sides in (14) be equal, we get 23

24. Let 24

25. Since ∈ ( −180°~ −70°), we can obtain the solutions of and. It is obvious that there are two groups of results to guarantee a small variable, we should choose one group according to “the shortest route”. 25

26. SIMULATION RESULTS 26 Figure 5. Initialized position of end-effector.

27. 27 Figure 6. Moved position of end-effector.

28. In experiments, we used the cine camera that fixed on robot to confirm the goal position of object as the vision orientation method. Then, robot picked up the goal object through the robot arm. 28

29. 29

30. CONCLUSION In this paper, the kinematics of the six-degree-of- freedom robot arm of humanoid robot was analyzed. The proposed method can obtain the kinematics equation at the same time with the geometric structure and the homogeneous transformation matrix to control the robot arm. 30

31. Finally, the paper presents the robot arm successfully pick-ups the goal (a bottle) utilizing Matlab Software to imitate the kinematics data and actual experiments. The experimental result verifies the validity of the kinematics equation. Next work, the robot arm system will apply to the order meal service robot, can be used for guest to receive, wait, order, check out, see a visitor out,… and so on in every restaurants or hotels in the early future. 31