Design, modeling, simulation and control of human inspired robotic arm

Yao Wang
Abstract In the traditional industrial robotic eld, the mechanical and control systems are usually designed for certain functions. They are planned to eciently complete one or several specic tasks, such as grasping, cutting and assembling. However, it is dicult to extend functions based on the industrial robotic hardware platform. Also, the broader applications of robot in the real world involve interaction with humans. Inspired from human shoulder and arm, we propose a six-degree of freedom robotic arm system. On the basis of the mechanical design inspired from human arm, it is possible to complete a variety of tasks only changing the control methods rather than hardware system. As a demonstration, the system is applied to complete three dierent family tasks: cutting food, cooking and cleaning the oor.

Introduction

With the development of robotic technology, robots are not only required to complete work in industry, but also in family. However, the core concept of industry task and family work are quite dierent. While the industry robots could eciently complete a single type of work and repeat hundreds of times in one day, the family robots are required to do dierent kinds of work, such as cleaning oor, cutting food and cooking, for once or twice in one day. As a result, the ability diversity is vital for family robots. Until now, the mainstream of family robots on the market is still designed with industrial concept. IRobot owns the products of vacuum cleaning robot Roomba, oor washing robot Scooba, pool cleaning robot Mirra, etc. All these products 1

Figure 1: Free Body Diagram. are single function family robots. Therefore, new kinds of robots for family environment have to be developed. We are developing a humanoid robotic arm. The robotic arm is composed of shoulder, elbow, wrist and manipulator and is expected to complete dierent family work with dierent control methods. In other words. this robotic arm is a hardware platform, which is software-scalable for dierent functions.

Design

Arm is one of the most exible parts of our body. To design and build a humanoid robotic arm with thinking in engineering, it is necessary to simplify the complex biomechanical model of human arm to a more feasible one. The free body diagram is show in Fig. 1. Fig. 1 shows that the human arm model here is simplied as a 6 degrees of freedom mechanical system. First, the shoulder part is capable of rotating around the three axes in the Cartesian space, which contains three degrees of freedom. Second, the elbow indicates one degree of freedom. Third, the forearm could rotate around the axis

Figure 2: Free Body Diagram with Angle Limitations. along itself respect to the elbow, which indicates another degree of freedom. Finally, the motion of end eector is combined with the wrist, preliminarily represented by one degree of freedom. Also it should be noticed that usually a degree of freedom has its limitation, known as conguration space. Generally speaking, no human joint can rotate more than about 200 degrees. These limitations are vital for us to determine the work space of the robotic arm. The free body diagram with angle limitations is shown in Fig. 2. Finally, after estimating the size of physical prototype, the 3D solid model could be built with Pro/ENGINEER, shown in Fig. 3.

Modeling

In this article, Lagrangian Equation is used to derive the equation of motion for the system. Since only rotational joints exist in the system, the homogeneous transformation can be reduced to rotational transformation. The rotation matrix can be computed as follows: 1 0 0 cos 2 sin 2 0 sin 2 cos 2 R1 = 0 cos 1 sin 1 , R2 = 0 0 sin 1 cos 1 0 0 1

Figure 3: 3D Solid Model.

 cos 3 0 sin 3 0 1 0 , R3 = sin 3 0 cos 3

cos 4 sin 4 0 sin 4 cos 4 R4 = 0 0 0 1

cos 5 0 sin 5 cos 6 sin 6 0 0 1 0 , R6 = 0 sin 6 cos 6 R5 = sin 5 0 cos 5 0 0 1 1 0 0 0 0 ,j= 1 ,k= 0 ,= 0 dene i = 0 0 1 0 Then the Jacobian matrix for angular velocity can be calculated: Jw1 = (R1 i, R1R2 , R1R2R3 , R1R2R3R4 , R1R2R3R4R5 , R1R2R3R4R5R6 ) Jw2 = (R1 i, R1R2 k, R1R2R3 , R1R2R3R4 , R1R2R3R4R5 , R1R2R3R4R5R6 ) Jw3 = (R1 i, R1R2 k, R1R2R3 j, R1R2R3R4 , R1R2R3R4R5 , R1R2R3R4R5R6 ) Jw4 = (R1 i, R1R2 k, R1R2R3 j, R1R2R3R4 k, R1R2R3R4R5 , R1R2R3R4R5R6 ) Jw5 = (R1 i, R1R2 k, R1R2R3 j, R1R2R3R4 k, R1R2R3R4R5 j, R1R2R3R4R5R6 ) Jw6 = (R1i, R1 R2 k, R1 R2 R3 j, R1 R2 R3 R4 k, R1 R2 R3 R4 R5 j, R1 R2 R3 R4 R5 R6 k ) Similarly, the Jacobian matrix for linear velocity Jv1 , Jv2 , ..., Jv6 can be computed. T Dene the vector of angular velocity = 1 2 3 4 5 6 . The kinetic energy T of the system can be written as follows: 1 T T Jvn + Jwn ( Ri)In ( Ri)T Jwn ]} T = T { [mn Jvn 2 n=1 i=1 i=1 In this equation, mn are the mass of the parts divided by joints and In are the corresponding inertia matrix. 5
6 n n

Then the potential energy could be computed as:

6

V =

0 1 0

n=1

(mn pn ) g

In this formula, pn is the mass center of dierent parts and g is the acceleration of gravity. The motion of equation can be got by substituting T and V into Lagrangian equation: L d L ( ) = dt i i In this equation, L = T V , is the generalized force. Finally, the motion of equation can be written as following format: + C (, ) + N (, ) = M ()

Simulation

In this article, ADAMS is selected as the simulation environment. ADAMS provides several numerical methods for simulation. Here the default integrator GSTIFF is chosen. It is a stiy stable, multi-step, variable order and variable step integrator and is fast and accurate for computing displacements for a wide range of motion analysis problems. As a backward dierence formulation, the general form of GSTIFF could be written as follows:
k

yn+1 =
1=1

i yni+1 + h0 yn+1

In the simulation, the order of the integrator is up to ve. Then the formulations are shown as follows: 1st order : yn+1 = yn + hyn+1 2 4 2nd order : yn+1 = 3 yn 1 y +3 hyn+1 3 n1 18 9 2 6 rd 3 order : yn+1 = 11 yn 11 yn1 + 11 yn2 + 11 hyn+1 48 16 3 4th order : yn+1 = 25 yn 36 y + y y + 12 hyn+1 25 n1 25 n2 25 n3 25 300 300 200 75 12 60 th 5 order : yn+1 = 137 yn 137 yn1 + 137 yn2 137 yn3 + 137 yn4 + 137 hyn+1 Test single joint input in ADMAS and apply a harmonic excitation on the rst joint, the response is shown in Fig. 4 to Fig. 9: 6

Figure 4:

Figure 5:

Figure 6: 7

Figure 7:

Figure 8:

Figure 9: 8

5
5.1

Control Design
Motion Planning

Some family tasks, such as mopping the oor and cooking, share some common features. In the process of completing these tasks, several joints of humans arm sustain xed angles and other joints move in periodic style, such as harmonic motion. These features are basis for more complex trajectory in the future application. Here the motion planning of mopping oor is executed as an example. Through examining the actual gesture of human when mopping the oor, the third and fth joints are set to sustain x angles in the mopping process. Dene i , i and i as the rotation angle, angular velocity and angular acceleration of each joint.(i = 1, 2, ..., 6) Then the parameters of xed joints can be written as follows: 3 = 0; 3 = 0; 3 = 0. 5 = ; 5 = 0; 5 = 0. 2 Through geometric analysis, parameters of the rst, second, fourth and sixth joints can be determined. The geometric relation of 2 and 4 is shown in Fig. 10 The range of 4 is from 0 to 114.55 , then it can be dened as follows: 1 = + cos( t) 4 12 2

In the Fig. 10, length of AB and BC are the length of forearm and arm. They are determined by the design parameters. The length of AC here can be computed as follows: |AC | = (|AB | + |BC |) 2 cos 1

As a result, 2 can be calculated from the following equation: sin(4 2 ) sin 2 = |BC | |AB | Similarly, the following equation can be used to obtain 1 if 2 is known: 9

Figure 10: Geometric Analysis.

|AC | = sin(4 = |AC | Finally, 6 can be derived from 1 :

(|AB |+|BC |) , 2 cos 1 sin 2 , |AB |

; .

6 =

1 2

Additionally, the angular velocities and angular accelerations can be computed from the angle function: i = i = di dt

di (i = 1, 2, ..., 6) dt As a result, the motion trajectory of the robotic arm can be completely determined for the task of mopping oor. Although the trajectory design for the working process has been completed, the motion planning problem can be more complicated if the initial condition is taken into consideration. Assume that the robotic arm is at rest initially.(The angles, angular velocities and angular accelerations are all zero) However, the initial conditions of the working trajectory is not all-zero. In 10

other words, there would be some step stage for several kinetic parameters of system. The total function of these parameters versus time is discontinuous at initial time, which aects the response and produces overshoots or oscillations. Since the robotics arm would work in the family environment, the extra action is not expected for safety of human. To eliminate these extra actions, it is necessary to nd the kinetic parameters of which the initial conditions are not satised with the function value of working trajectory at time zero. To solve this problem, an extra stage of lift is designed, to set the initial conditions of certain joints to the desired values over two seconds. Then the angle functions of all the joints can be written as follows: 1 = 3 2 t + t; 12 4 2 = 0;

3 = 0; 4 = 0; 3 2 5 = t3 + t ; 2 = t3 t2 ; 8 8 24 8 Then the whole motion planning is nished.

5.2
5.2.1

Trajectory Tracking
Computed Torque Method

The dynamics of the robotic arm can be expressed in the following form: + C (, ) + N (, ) = M () Given a desired joint trajectory d (), assume that the model is precise and initial conditions are exactly the same: (0) = d (0); (0) = d (0)

The control torque can be chosen as follows: d + C (d , d ) d + N (d , d ) = M (d ) Since and d have the same initial conditions and satises the same dierential equation, then from the uniqueness of dierential equation it can be shown that (t) = d (t) for all t 0).

11

This is an open-loop control law and the robustness cannot be ensured. Here a state feedback is added into the control law: d + C (d , d ) d + N (d , d ) F (Kv e = M (d ) + Kp e) In this equation, Kp and Kv are proportional and derivative coecient matrices. They are both constant matrices. F is a time-varying matrix to de determined. However, the motion equation of the system should be linearized around the designed trajectory before the specic structures and values of matrices Kp , Kv and F can be determined. It is assumed that the error between actual motion and desired trajectory is small enough throughout the whole process of movement. Then the nolinear parts in the equation can be linearized as follows: M () = M (d ); It follows that: d ) + F Kv ]e M (d ) e + [C (d , + F Kp e = 0 Then the robotic arm model is converted to a linear time-varying system, Since the initial conditions are ensured that e(0) = e (0) = 0, the objective is to make the system internally stable through selecting appropriate F , Kp and Kv , to guarantee that e(t) = e (t) = 0 for all t 0. From the linear control theory, it is known that a time-varying equation is asymptotically stable if (t, t0 ) 0 as t ) = C (d , d ); C (, ) = N (d , d ) N (,

for all t, t0 with t t0 . (t, t0 ) is the transition matrix. One procedure of selecting the matrices F , Kp and Kv is as follows: (1) Write the matrix F to be a diagonal matrix with the diagonal elements as the diagonal elements of the matrix M (d ). (2)Select appropriate constant diagonal matrices Kp and Kv to make the system track the desired trajectory. (3)Adjust Kp and Kv to eliminate the oscillation and errors. Through this design procedure, some tasks and applications which dont require to move very fast can be completed. The simulation result of a trajectory concerning harmonic motion is shown in Fig. 11. 12

Figure 11: Simulation Result.

13

However, several drawbacks limit the eects in the actual application of this method. First, this method is not capable of tracking high speed tasks. For instance, if the frequency of the harmonic motion in Fig. 11 is set as three times higher, the simulation result would greatly oscillate. Second, it is dicult to nd general forms or rules of F , Kp and Kv those are applicable to all kinds of trajectory, which is extremely vital in the design of multifunction robotic arm. Therefore, an improved computed torque method is introduced. 5.2.2 Improved Computed Torque Method

Consider the rened open-loop control law: d + C (, ) + N (, ) = M () Substituting this control law into the dynamic equations of robotic arm, it can be seen that: = M () d M () Since M () is positive denite for in , we have: = d It means that if the initial angles and angular velocities of the robotic arm match the desired ones, the arm will follow the desired trajectory. Similarly, the state feedback is needed to correct for the initial condition errors. The closed-loop control law can be expressed as follows: d Kv e ) + N (, ) = M ()( Kp e) + C (, Where e = d and Kv , Kp are constant gain matrices. When substituted into the dynamic equation of the robotic arm, the error dynamics can be written as: M ()( e + Kv e + Kp e ) = 0 Since M () is positive denite, we have: e + Kv e + Kp e = 0 Then the nonlinear system is converted to a linear one: d dt e e = 0 I Kp Kv 14 e e

Figure 12: Simulation Result of Improved Computed Torque Method. Dene A= 0 I Kp Kv

It is easy to change the eigenvalues of matrix A with selecting appropriate gain matrices Kv and Kp . Then the behavior of the system response can be controlled. The simulation result of a motion with high speed at certain time is shown in Fig 12. In Fig 12, red curve, blue curve and green curve represent the desired trajectory, actual trajectory and error, respectively.

5.3

Force Control

In the above control design process, only trajectory tracking is considered in the mopping application of the robotic arm. However, forces exerted by the constraints must be taken into consideration as well. For instance, a mopping oor application requires the end eector of the robotic arm to exert a constant distributed normal force to a plane, as shown in Fig. 13. Supposed that A and B are the endpoints of the end eector. Then the oor can be described by a set of independent constraints. In our example, assume that the space coordinates of point A and B are (xA , yA , zA ) and (xB , yB , zB ). The oor plane is represented as y = y0 . (y0 is a constant).

15

Figure 13: Mopping Floor. Then the surface constraints can be written as: h1 (1 , 2 , ..., 6 ) = yA (1 , 2 , ..., 6 ) y0 = 0 h2 (1 , 2 , ..., 6 ) = yB (1 , 2 , ..., 6 ) y0 = 0 Now the oor surface is represented as the function h of the generalized coordinates 1 , 2 , ..., 6 . Hence, the normal vectors to this surface are given by the span of the gradients of h. As a result, any torques with the form N = j hj

correspond to normal forces applied against the oor surface. If the friction is not taken into consideration, the work done by these torques can be computed as follows: hj = = ) N j hj j ( dh = j =0 dt It means that the normal forces have no eect on the motion of the robotic arm. In the previous trajectory tracking parts, the computed torque has be attained to follow the desired motion. Now if we consider the force control, the complete control law is given by: c = + N Since the torques N do not aect the trajectory tracking part, their function is to exert desired forces to the oor surface.

16

Summary

In this article, modeling of the robotic arm is completed with Lagrangian method. Then the simulation of the system motion is base on the GSTIFF numerical method. In the control design method, the computed torque method is rst used for trajectory tracking. However, due to the limitation of application in the high speed motion, the improved torque method is introduced for the position control. Finally, the basic force control is introduced to deal with the environmental forces. In the future work, control strategies for the system with the actuators, such as motors and sensors, should be developed for the actual application. Also the physical prototype would be built in the future.

References
[1] M. Tomsic N. Klopcar and J. Lenarcic. A kinematic model of the shoulder complex to evaluate the arm-reachable workspace. Journal of Biomechanics, 40:8691, 2007. [2] Goran Sigholm Christian Hogfors and Peter Herberts. Biomechanical model of the human shoulder-i. elements. Journal of Biomechanics, 20:157166, 1987. [3] Mahmoud A. Moustafa Khaled T. Mohamed, Shihab S. Asfour and Hasan A. Elgamal. A computerized dynamic biomechanical model of the human shoulder complex. Computers ind. Engng, 31:503506, 1996. [4] Walter Maurel and Daniel Thalmann. Human shoulder modeling including scapulo-thoracic constraint and joint sinus cones. Computers and Graphics, 24:203218, 2000. [5] H. Bao and P.Y. Willems. On the kinematic modelling and the parameter estimation of the human shoulder. Journal of Biomechanics, 32:943950, 1999. [6] DAN KARLSSON and BO PETERSO. Towards a model for force predictions in the human shoulder. Journal of Biomechanics, 25:189199, 1992.

17

[7] Guangpin He Xiguang Huang and Duanling Li. Kinematics analysis of a parallel robotic manipulator. Applied Mechanics and Materials, 2932:956960, 2010. [8] Xiaoping Su Dan Zhang, Jijie Qian and Zhen Gao. Design, analysis and fabrication of a novel three degrees of freedom parallel robotic manipulator with decoupled motions. Int J Mech Mater, 9:199212, 2013. [9] Anjan Kumar Swain and Alan S. Morris. A united dynamic model formulation for robotic manipulator systems. Journal of Robotic Systems, 20:601620, 2003. [10] Thomas R. Kane and David A. Levinson. DYNAMICS: Theory and Applications. McGraw-Hill Book Company, 1985. [11] Seth Hutchinson Mark W. Spong and Mathukumalli Vidyasagar. Robot Modeling and Control. Wiley, 2005. [12] Zexiang Li Richard M. Murray and S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [13] Kemin Zhou. ESSENTIALS OF ROBUST CONTROL. PRENTICE HALL, 1999.

18

Appendix
Name Twin Geared Motor- Tamiya 70097 Kitbots 1000 RPM Gearmotor Quantity 3 Website http://www.robotmarketplace.com/products/0-70097.htm l http://www.robotmarketplace.com/products/KB-1000RP M.html http://www.robotshop.com/en/pololu-dual-dc-motor-driv er-1a-4-5v-3-5v-tb6612fng.html Cost($ ) 35.1

3 3

48 14.85

Pololu Dual DC

Motor Driver 1A,

4.5V-13.5V-

TB6612FNG

1 Arduino Due

http://banebots.com/pc/WHB-HM-HS6-S4/T82H-SS42

49.95

32bit ARM

Microcontroller

AS Series 360 3.3 to 5 V SMT Programmable Magnetic Rotary Encoder SSOP-16 BMI055 Series 3.6 V 5 mA Small Veratile 6 DoF Sensor Module -

http://www.futureelectronics.com/en/technologies/semic onductors/analog/sensors/gyro-angular-rate/Pages/36503 14-AS5045-ASST.aspx?IM=0

36.42

http://www.futureelectronics.com/en/Technologies/Produ ct.aspx?ProductID=BMI0550273141134BOSCH7035254&I M=0

20

LGA-16 Multipurpose 6061 Aluminum, 1/2" Thick, 2" Width, 3' Length Pan Head Phillips Machine Screws(Part Number: 91735A833) 1 http://www.mcmaster.com/#standard-aluminum-sheets/= py9kwq 22.7

1 packs of 25

http://www.mcmaster.com/#machine-screw-fasteners/=m c86h3

6.39

3 9V Batteries Quantity 2 12V battery Quantity 3 9V battery connector 3 2-pack Clip Holder Box 4 Energizer 9V battery Battery Holder Box Case 4

http://www.amazon.com/gp/product/B000K2NW08?tag=v iglink129224-20

28.95

http://www.amazon.com/gp/product/B006IBWSLY?tag=vi glink129224-20

12

http://www.amazon.com/gp/product/B0002ZPFU8?tag=vi glink129224-20

11.67

http://www.amazon.com/gp/product/B006SQ50VY?tag=vi glink129224-20

3.15

Dollar General

17.12

http://www.amazon.com/5Pcs-Battery-Clip-Holder-Black/d p/B00ABRQYPM/ref=pd_bxgy_e_img_y

4.16