2013 Murakami Lab.

Simulation Assignments

1 Task1Motion Control of A Motor 1.1 Modeling of The Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 1.3 1.4 2 PD Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Integral Approach in The C Program . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Realization of As1 +B in The C Program . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 2 3 4 5 6 7 7 7 7 8 9

Task2Control of a two links manipulator 2.1 Dynamics of the two link manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.1 2.1.2 General form of Lagrange equation . . . . . . . . . . . . . . . . . . . . . . . . . . . Modelization of the two link manipulator . . . . . . . . . . . . . . . . . . . . . . .

2.2

2.1.3 Derivation of Lagrange function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.4 Derivation of dynamics equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 2.2.2

Direct kinematics of the 2 link mainpulator . . . . . . . . . . . . . . . . . . . . . . 9 Inverse kinematics of the 2 link manipulator . . . . . . . . . . . . . . . . . . . . . . 10

2.3

Contorl method using inverse Jacobian matrix . . . . . . . . . . . . . . . . . . . . . . . . 11

Task1Motion Control of A Motor

According to explanation below, do simulations about motion control of a motor. Please describe graphs with response on the y-axis and time on the x. Then change the position gain Kp and the velocity gain Kv and consider the behavior of the response. Please hand in .c File , Graphs . The deadline is April 22nd.

[ Assignment 1 : Motion Control of A Motor] Do motion control of the motor using PD controller. The input to the motor is step and sinusoidal wave. Please change the position gain Kp and the velocity gain Kv and simulate Over Dumping , Under Dumping and Critical Dumping respectively. In the simulations, use parameters below. Jm : D: Kt : Gr : Step Input : Sinusoidal Wave : Sampling Time : 0.0000040 [kgm2 ] 0.0000040 [Nm rad/s] 0.156 [Nm/A] 100.0 cmd = 1.0 [rad]
cmd

(t > 1.0 s)

= sin(t) [rad] 1 [ms]

1.1

Modeling of The Motor

A motion equation of the motor in g.1 is shown as below. Jm a Tm a T = Tm Da Tdis = K t Ia = Gr = Gr Tm (1) (2)

In the expressions above, each symbol denotes the meaning below. a [rad] Jm [kgm2 ] D[Nmrad/s] Tm [Nm] Kt [Nm/A] Ia [A] Tdis [Nm] [rad] Gr :Rotation Angle of The Motor :Inertia Moment of The Motor :Dumping Coecient :Motor Torque :Torque Constant :Armature Current :Disturbance Torque :Rotation Angle of The Output :Reduction Ratio

Motor

Output Axis

1: Modeling of The Motor

If Tdis can be ignored in (1) , a transfer function from Ia to is shown as below. = Gr Kt Ia (Gr Jm Gr s + Gr DGr )s (3)

A block diagram of the motor including disturbance torque is shown in g.2.

Tdis
Ia

Gr K t

& res 1 res 1 Gr JGr s + Gr DGr s

2: Block Diagram of The Motor

1.2

PD Control

Fig. 3 shows PD control system.

cmd+

KP

res &res &&ref Tref Gr Kt 1 Ia 1 + Gr JmGr + Gr JmGr s +Gr DG Gr Kt s r

&cmd +

KV

KV

3: PD Control System

1.3

Integral Approach in The C Program

In this section,two approaches of integration are shown as examples. 1. Integration using rectangle : A Conceptual diagram is shown in g.4.

4: Integration Using Rectangle

2. Trapezoidal Integration : A Conceptual diagram is shown in g.5. Of course, this method is more accurate than the former.

5: Trapezoidal Integration

1.4

Realization of

1 As+B

in The C Program

The transfer function

1 As+B

can be changed to expression below. Y Ay y = 1 X As + B + By = x 1 = (x By ) A

1 A (x

(4) (5) (6)

(6) indicates that y can be obtained by integrating

By ).

1 A

& y

1 s

6: Realization of

1 As+B

in The C Program

Do a simulation of the motion control of the motor using integration approach above without disturbance.

Task2Control of a two links manipulator

Submit plots of the simulation results about two links manipulator which is shown in Fig.7. There are four simulations which are written in under the box. In addition, submit the source code only the case which the command trajectory is circle and the control method uses disturbance observer. Write speculations about each simulations. If you dont know about disturbance observer, you should read the paper which is written about disturbence observer by Professor Kouhei Ohnishi. Deadline is April 29th.

[ Task2 : Control of a two links manipulator] Compare some simulations by changing command trajectories at the tip and control methods.

Control method Command trajectory of the tip 1.Step input10cm) 2.Circular orbit radius is 10cm) 1. The method of using inverse Jacobian matrixFig.8. 2. The method of using disturbance observer in addition to method 1 Each parameters are as follows l1 : 0.5[m] l2 : 0.5[m]

m1 : m2 :

1.0[kg ] 1.0[kg ]

Constraints about programming

Drawing step input and circular orbit by dot-line and drawing reponse value by solid line. Write main variables based rules which are as follows

x i dx i Command(of the x) x cmd

x ref

x i ddx i i th i Response(of the x) x res Reference (of the x)

Write variables of each joints by using structures or arrays Represents disturbance observer using a function

A few words of caution There are some points wich are easy to mistake Using integration in a particular orderFirst, calculate positions from a velocities. Then calculate velocity from accerelation. When using disturbance observer, add current command to compensation current which was calculated 1 sampling before. (You cant use present values for calculating compensation current.)

Extra simulaiton If you have enough time, do extra simulations and improve your skil. If you have some questions, ask someone please.
Do simulations considering disturbance as same as Task1 Compare the simulation results when controller is changed(only PD controller and PD controller

+ disturbance observer) under the condition that there are disturbances.

Compare the simulation results by adding I controller or accerelation feedforward

2.1

Dynamics of the two link manipulator

There are two kinds of dynamics. One is the direct dynamics and another is the inverse dynamics. Direct dynamics is the method for calculating positions, velocities, and accerelations in each joints from joint torques or positions. Inverse dynamics is the method for calculating joint torques which are needed for controlling manipulator from positions, velocities, and accerelations in each joints. Direct dynamics is needed for simulations as a representation of robot motions. Inverse dynamics is needed for control input of the robots. Generally, equations of motion are very complex so less aected terms are ignored when the robots have to be controlled real time. Disturbance observer compensates these ignored terms. Equations of motion(equations of dynamics) can be derived by Lagrange equation or NewtonEulers equation. In this simulation, we use Lagrange equation.

2.1.1

General form of Lagrange equation

In this simulation, we derive equations of motion by using Lagrange equation. Lagrange function L is dened as the dierence between motion energy K and potential energy P L=K P Equation of motion is derived from Lagrange equation ( ) d L L Fi = dt qi qi qi is generalized coordinate that represents each energy. Fi is the power or torque depend on qi (7)

(8)

2.1.2

Modelization of the two link manipulator

Describes a modelization of the two link manipulatorIn this simulation, link mass is considered as mass point at the tip as shown in Fig.7. l1 ,l2 are the length of each linksIn addition, 1 and 2 are the angle of joints

\ O

 P

O

P

7: 2 link manipulator

2.1.3

Derivation of Lagrange function

Motion energy of the link 1 is represented as follow equation. K1 =

2 1 m1 l1 2 1 2

(9)

Potential energy is derived by follow equation. P1 = m1 gl1 cos 1 (10)

The tip coordination of the 2nd link is dened as (x, y ). Relationship between 1 and 2 can be represented by next equations x = l1 sin 1 + l2 sin(1 + 2 ) y Then, tip velocities are as follows x = y = (13) (14) = l1 cos 1 l2 cos(1 + 2 ) (11) (12)

From the velocity, motion energy can be calculated as follows K2 = (15)

Potential energy can be calculated as shown in equation(19) because height of the 2nd link is represented as shown in equation(14) P2 = Then, Lagrange function L = K P can be derived by following equation. L= (17) (16)

2.1.4

Derivation of dynamics equation

Calculate Lagrange equation to derive dynmics equation L = 1 ( ) d L = dt 1 L 1 =

(18)

Then, torque of the motor at joint1 is calculated as shown in equation (22) T1 = + + ((m1 + m2 )l1 2 + m2 l2 2 + 2m2 l1 l2 cos 2 ) 1 (m2 l2 2 + m2 l1 l2 cos 2 ) 2 2m2 l1 l2 sin 2 1 2 m2 l1 l2 sin 2 2 (m1 + m2 )gl1 sin 1 + m2 gl2 sin(1 + 2 )
2

(19)

Calculate torque of the motor at joint2 by using same order of joint1 L 2 d L dt 2 L 2 =

(20)

2 T2 = (m2 l2 2 + m2 l1 l2 cos 2 ) 1 + m2 l2 2

+m2 l1 l2 sin 2 1 + m2 gl2 sin(1 + 2 )

(21)

Euation(19) and (21) can be rewritten as equation (25). + H + G T = M (22)

T = M

[T1 T2 ]T , = [1 2 ] ] [ ] [ 2 M11 M12 H122 2 + H112 1 2 , = , H= 2 M21 M22 H211 1

[ G=

G1 G2

In inertia matrixM, Mii is a self-inertia coecient, and Mij is a mutul-inertia coecientIn nonlinear i j is the term of KolioriG is the term of gravity termHHiij

2.2

Kinematics

Kinematics represents the relationship between tip position and angle, angle velocity and angle velocityThere are direct kinematics and inverse kinematicsDirect kinematics can be used to calculate tip position from each jointsOn the other hand, inverse kinematics can be used to calculate each joints angle from the tip positionIn addition, inverse kinematics is important for trajectiry planning and kinematics is used to exchange command trajectory for joint space.

2.2.1

Direct kinematics of the 2 link mainpulator

In Fig.7, the relationship between joint angle and tip position of 2nd link can be represented by (23) (24) x = l1 sin 1 + l2 sin(1 + 2 ) y = l1 cos 1 l2 cos(1 + 2 ) (23) (24)

By using derivation, relationship between velocity and joint velocity can be represented as equation (28) [ ] [ ][ ] x 1 = (25) y 2

2 2exchange matrix in equation (28) is called Jacobian matrixEquation (28) can be represented as follow equation and it can be applied for multi degrees of freedom manipulator x = Jaco (26)

Then, by derivation of equation(26) , equation which represents relationship between joint velocity and tip accerelation can be derived + J aco = Jaco x [ ] = [ ][ ] + [ ][ ] (28) (27)

x y

1 2

1 2

2.2.2

Inverse kinematics of the 2 link manipulator

Equation(29) can be derived from Fig.7. by using identities for cosine. x2 + y 2 = l1 2 + l2 2 + 2l1 l2 cos 2 Then, 2 = [ 1 = arctan [ ] arctan l2 sin 2 l1 + l2 cos 2 ] (31) (30) (29)

At inverse kinematics problem, the solution is non-unique. at this time, 1 has two solutions If there is a Jacobian matrix at equation (26) , following equation can be derived by multiplying inverse Jacobian matrix to both sides from left. [ ] [ ][ ] 1 x 1 = (32) 2 x 2 From equation (27) , accerelation can be derived = J1 ( aco x Jaco ) [ 1 2 ] = (34) (33)

Generally, 2nd term of the inverse kinematics at accerelation dimension can be ignored, so equation (37) can be rewritten as equation (38). = J1 ( (35) aco x)

10

2.3

Contorl method using inverse Jacobian matrix

Block diagram of whole control system is shown in Fig.8. PD controller is used as a controller as shown in Fig.9. To control manipulator, calculate the error between command trajectory and actual trajectory. Then, exchange to joint space by using inverse kinematics(inverse jacobian matrix) to make control input.

x cmd

x ref
Controller
1 J aco

ref
Mn
(Gr Ktn )1

Ia

res
Inverse Dynamics

Trajectory Planning
x cmd

x res x res
J aco

res

Direct Kinematics
8: Control system of the 2link manipulator

x cmd

KP

+ +

x ref

x cmd x res

+
res x

KV

9: PD controller

At this system, input for plant(manipulator) is accerelation as shown in Fig.10. so this is accrelation control system. represents parameters which are nominal valueNominal values are diagonal elements of the matrix. At this simulation, torque constant is non-variable and considered as Ktn = Kt There are model error and parameter variations at this system.

Tdis

ref
Mn

(Gr Ktn )1

Ia
Gr Ktn

res
M 1
Robot

1 s

res

res 1 s

10: Accerelation control system

11