Upload
jonecncjone
View
226
Download
0
Embed Size (px)
Citation preview
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
1/14
TWO DOF FLEXIBLE LINK EXPERIMENTQuanser Consulting Inc.
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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.
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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.
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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/2019 Rotary - 2D Flexible Link (SRV02) A
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)
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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];
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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.
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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.
8/14/2019 Rotary - 2D Flexible Link (SRV02) A
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