Rotary - 2D Flexible Link (SRV02) A

8/14/2019 Rotary - 2D Flexible Link (SRV02) A

1/14

TWO DOF FLEXIBLE LINK EXPERIMENTQuanser Consulting Inc.


2/14

Home Position

1.0 Introduction

The system is shown in the photograph.Two motors drive a gearbox and an externalgear which in turn drives a flexible link

instrumented with strain gages. The flexiblelinks are attached to rigid links using highquality low friction joints. The two rigid linksare also coupled via another joint resultingin a 5 bar linkage. The robot has twodegrees of freedom and 2 flexible degreesof freedom. The intention is to developcontrollers that position the tip of link 5while reducing oscillations due to theflexibility in the structure and to reducecoupling effects between the joints.

Note that Link 2 and Link 3 are essentiallyone link with a joint in the middle.The home position of the robot is shown

above


3/14

2.0 System parameters

Parameter Symbol Value Units

Motors

Torque constant Km .0109 Nm/Amp

Resistance Rm 1.6 Ohm

Gear ratio Kg 91 -

Encoder resolution 8192 Counts/rev

Encoder gain 0.0439453125 Deg/count

Gain cable gain 3 V/V

Flexible LinksLength L .23 m

Mass mf .09 Kg

Collocated Stiffness Kstiff 7.5 Nm/rad

Rigid links

Length L .23 m

Mass ml .08 Kg

Joint masses

Elbow mj1 .03 Kg

Tip mj2 .04 Kg

Strain gages

Gain (linear) 1 cm/V

Gain (angular) 2.5 deg/volt


4/14

TITLE:

DRAWN BY:

DATE:

2DFLE CABLING

99/09/15

T. ACKLAND

S1 & S2 S3 S4S1

S2 S3S4

G

OUT

G

FROMANALOGSENSORS

Test Points To A/D

To Load

Power Amplifier

-

+From D/A

Power Supply

+Vs G -VsUPM-X

UPM-Y

To LoadFrom D/A

S1 & S2

SENSORSANALOG

FROM

Power Amplifier

S3

+

- OUT

G

S4

Power Supply

+Vs

Test Points

G -Vs

To A/DGS3S2

S1 S4

FROM STRAIN

GAUGE X

FROM STRAIN

GAUGE Y

TO MOTOR X

TO MOTOR Y

0

1

3

2

7

6

5

4

FROM ENCODER X

FROM ENCODER Y

TO ANALOGINPUTS #0,1,2,3

FROM ANALOG

OUTPUT #0

FROM ANALOGOUTPUT #1

ANALOGINPUTS

ANALOG OUTPUTS

E

N

C

O

D

MULTIQ-3TERMINAL BOARD

0 1 2 3

7654

0123

5 467

TO UPMAMP Y

TO UPMAMP X

X(3 LINKS)

Y

TO ENCODER #0

TO ENCODER #1

FROM UPMLOAD Y

LOAD XFROM UPM

FROM "TO A/D" X

TO S3 ON UPM-X

TO S4 ON UPM-X

2DFLE

S4 S3 S2 S1

(2 LINKS)

3.0 Assembly and wiring

Wire the system as shown.


5/14

This wiring results in:

D/A0 Amplifier X input Motor X

D/A1 Amplifier Y input Motor Y

Encoder X Encoder # 0 on MultiQ

Encoder Y Encoder # 1 on MultiQ

Strain gage X S2 A/D # 3 on MultiQ

Strain gage Y S3 A/D # 4 on MultiQ


6/14

4.0 Testing

Load the WinCon project q_test_2dof.wcp using File/Open in WinCon. This displays

the screen shown.

The top two displays are the measurements from the strain gages. You may need toadjust the offset potentiometers to obtain approximately zero from these. Do this byturning the potentiometers on the strain gage circuits shown.

The next two displays are for the joint angle. Pulling the end effector towards x+ and y+should results in positive angles in both measurements.

You can also apply small voltages to the motors using the slide bars. A positive voltageshould result in a positive angle.


7/14

90

EndEffector

Y MotorLinks

X MotorLinks

10 inches

20 inches

X

Y

Home

(X+,Y+)

Home position and the (x+,y+) direction

To test:

1) Move the robot to the home position. Click start. If the strain gage measurementsare nonzero, tune the Offset adjustpotentiometer to obtain zero. DO NOT TOUCH thegain potentiometer.

2) Pull on the end effector towards (x+ y+) is in the direction shown below. The anglesshould increase in the positive direction.

3) Disturb the end effector lightly. You should see oscillations in the plot showing thedeflections.

4) Slide the voltage slide bars and see the links move in the correct directions.


8/14

X MotorLinks

M1+dx Bx (T1)

LinksY Motor

M2+dy

T2

Same point

Same point

5.0 Mathematical models

The Home position of the robot is defined as the position when the angles betweenthe links is 90 degrees and with link 3 parallel to the base. This position is (20,10)inches from the base coordinate frame.

The X motor causes a rotation M1 and the link attached to it causes a deflection dx.Together they form the rotation angle of link #1. The same applies to the Y motor whichrotates an angle M2 and the link attached to it deflects by dycausing a total rotationangle of the link. Assuming all links are rigid, then the parallelogram structure requiresthat the other two joint angle follow a prescribed path.

5.1 Kinematics

The forward kinematics are derived assuming rigid links using the MAPLE script fileinverse.map. The results are saved in forward.eqn and in inv_sol.m andinv_sol.map.

The forward kinematics are:

PxX ' la cos(2x&$x) % la sin(2x)

PyX ' la ( sin(2x&$x) &la cos(2x)


9/14

where these are the X and Y positions of the tip relative to the base coordinate frame.

2x is the angle (M1+dx) of the X motor and the link deflection and $x is the angle Bxshown in the figure above. Although Bx is not measured it can be obtained directlyfrom M1 & M2.

Note that due to the four bar linkage the following relationships apply:

Given motor angles Mx and My, then

T1 ' Mx%MyT2 ' Mx%My

The inverse kinematic equations assume rigid links and drop dx and dy to zero. Theyare much too complex to print here.

Both equations are implemented in the S functions rob2d_f.cand rob2d_i.cand areembedded in the final controller. A section of code that performs the inversekinematics is shown below.

if(Y >0)

{

t_y = asin((X*X+Y*Y-5*l*l)/l/l/4);

a = 4*l*sin(t_y)+2*l;

b =

4*l*l*sin(t_y)*sin(t_y)+4*l*l*sin(t_y)+l*l-X*X+4*l*l*cos(t_y)*cos

(t_y);

if(b >= 0)

{

t_x = 2*atan((a-2*sqrt(b))/2/(X+2*l*cos(t_y)));

*y1 = t_x;

*y2 = t_y;

}

else

{

*y1 = 0;

*y2 = 0;}

}

if((Y 0))

{

X = -*uPtrs2[0];

Y = *uPtrs1[0];


10/14

t_y = asin((X*X+Y*Y-5*l*l)/l/l/4);

a = 4*l*sin(t_y)+2*l;

b =

4*l*l*sin(t_y)*sin(t_y)+4*l*l*sin(t_y)+l*l-X*X+4*l*l*cos(t_y)*cos

(t_y);if(b >= 0)

{

t_x = 2*atan((a-2*sqrt(b))/2/(X+2*l*cos(t_y)));

*y1 = t_x+pi/2;

*y2 = t_y;

}

else

{

*y1 = 0;

*y2 = 0;

}}

if((Y


11/14

M1+dx

F_U_0

M2+dy

T2

T1

F_U_J2

F_U_J1F_U_J3

F_U_0

F_U_J3

F_U_J2

T1cg1

T1j1

T2cg2T2j2

T2cg3

T2Load

T2cg4

T2j3

T2cg5 T2j4

5.2 Dynamics

The dynamics are developed using Lagrangian dynamics. The position and velocity ofeach element is derived using transformation matrices and used to compute thepotential and kinetic energies. These are then solved for the accelerations which are

then linearized to obtain a linear model.

The MAPLE file twdof.map derives the differential equations and save the linearmodel into twodof_m.m

The coordinate frames used in the Maple script are shown below


12/14

a n s e r C o n s u l t i

M u l t i Q T i m e r

T i m e - B a s e

S u m

A n g l e s 2

S u m

A n g l e s 1

S u m

A n g l e s

S e l e c t

r o b 2 d _ f

S - F u n c t i o n

Py c

Py

P x c

Px

0

N o J o i n t f e e d b a c k

M u x

M u x

Full Feedback

Cmd 2

Vy

M 2

dy

M2 d o t

dy dot

Total y

M o t o r 2

2 l inks

Full Feedback

Cmd 1

Vx

M1

dx

M1 dot

dx dot

Total X

M o t o r 1

3 l inks

K

M a t r i x

Ga in

K-

G a i n 1

K-

Ga in

1

F u l l f e e d b a c k

Ey

Ex

D u r a t i o n

D e m u x

M 1 C m d

M 2 C m d

X C m d

Y Cmd

C o m m a n d s

6.0 Control System design

The controller is designed using LQR in the m file d_twodof.m. the program alsogenerates a command path for the robot to track. Setting the variable path ind_twodof.m generates three types of trajectories (1 = 2 circles, 2 = square, 3 = funny

house)

7.0 Results

The controller is implemented in Simulink and run using WinCon. The controller is

q_2dof_sg.mdl which can be recompiled for WinCon.


13/14

To test our controller simply start WinCon and load the project q_2dof_sg.wcp. Thismakes the end track a the funny house trajectory command and stops at the end ofthe trajectory. The windows that pop up are shown below.

You can run the controller with or without feedback by clicking on the button in thecontrol panel window.


14/14

The plot below shows the response of the system with and without full feedback. You

can clearly see the improvement using feedback from the strain gages

Rotary - 2D Flexible Link (SRV02) A

Documents

Transcript of Rotary - 2D Flexible Link (SRV02) A