Sie sind auf Seite 1von 81

# Design Of A Digital Control System And

## Path Planning Module For The ED-7220C

Robot Arm

Presented by:
BOUAMER Tarek
ELGHERIBI Abdellah Nabih

## Supervisor: Dr. R. GUERNANE

Institute of Electrical and Electronic Engineering IGEE (Ex-INELEC) 2014/2015

Outlines

General Introduction
Robot Kinematics
Drive Circuit
Digital Controller
Robot Control
Trajectory Planning
ROS
Motion Planning
Conclusion

Introduction

## ED-7220C Robot Arm

Our Goal
Replacing the drive/control box and software of ED-7220C Arm with
an open one
Steps :
Mathematical analysis for the forward and inverse kinematics of the
robot.
Design the Actuators Power drive system.
Design a digital position controller for the robot arm joints.
Design an interface to generate and simulate robot trajectories for
pick-and-place task in a cell environment with obstacles.
Integrate the interface with the robot control system.

Robot Kinematics
Kinematics is the description of motion without regard to the forces that cause it.
Kinematics analysis is divided into:
Direct/Forward Kinematics
F(1,2 ,..,n ) = [x, y, z, R]
Inverse Kinematics
F(x, y, z, R) = 1, 2,, n

Direct/Forward Kinematics
Tii1 = Rot (z, i) Trans (z, di) Trans (x, ai) Rot (x, i)
Where the four quantities i , ai , di , i are the parameters of link i and joint i.

1 = 1 ( )
1

=
0
0

=
0

0
0
0 =
0

1
1

0
1
0
1

0 1 , 2 , = 10 (1 ) 21 2 1 ( )
1 is a homogenous transformation matrix of the coordinates of frame i to frame i-1
0 is a 3x3 rotation matrix
0 is a three dimensional vector

## Assigning the Coordinate Frames

ED-7220C DH Paramters
Table (1.1): DH parameter for ED-7220C robot arm

## Table (1.2): The link lengths

Using the paramters from table (1.1) the matrix T1 to T6 can be obtained as shown
below

0
0 1 2 3 4
5 = 1 2 3 4 5 =

= 50

## X, Y and Z resultants gotten from forward kinematics

after multiplying the Homogenous matrix by joint
variable

Inverse Kinematics
1. Geametric Approach
2. Analytical (algebraic) Approach

Geametric Approach
1= atan2(y, x)
= 2 + 3 + 4
4 = 5 cos()

4 = 5 sin
= 2( 2 +22 32 , 22 s)
= 2 4 1 , 4
2 =

3 = 2 2 22 32 , 22 3
4 = 2 3

## Planar view of ED-7220C

Drive Circuit
Enable Pins Encoders Motos Direction

Limit Switches

## Connections to ED-7220C Arm

Bridge Diodes
L298 H-bridge

Digital Controller

## Arduino Due Summary

Operating Voltage
Input Voltage (recommended)
Input Voltage (limits)
Digital I/O Pins
Total DC Output Current on all I/O lines
DC Current for 3.3V Pin
DC Current for 5V Pin
Flash Memory
SRAM
Clock Speed

3.3V
7-12V
6-16V
54 (of which 12 provide PWM output)
130 mA
800 mA
800 mA
512 KB all available for the user applications
96 KB (two banks: 64KB and 32KB)
84 MHz

Robot Control
Control Techniques

Coupled control
a manipulator with n DOFs can be described in
coupled control :

+ , + =
: Inertia matrix; Symmetric Positive Definite
matrix.
: Gravity vector from the inertial frame.
: Coriolis and centripetal matrix.

## The system is considered as one box of an

interconnected chain of ideal rigid bodies with
a generalized force acting at the joints.
Full state information for each of the n joints is
available to calculate the controls for each
joint.

Extremely complicated and difficult to determine the
parameter of the equation.
Dynamic effects are not taking consideration in the equation:
elastic deformation of bearing and gears.

Decoupled control
The manipulator is controlled as (SISO)
system.
Any coupling effect due to the motion of the
Disturbance D
The force of gravity acting on the Links.

Design of Controllers

Feedforward compensator.

( 0).

## To reject and cancel the

disturbances introduced on

No overshoot (critical
damping).
PI compensator

Shoulder
Elbow

PI compensator
Tuning the PI parameters using experimental
iterative technique.
Procedure:
1) Set the Integral and derivative gains to zero.

## 2) Tune the Proportional gain (Kp) to improve

the transient response, neglecting the steady
state error.
3) Adjust the integral gain (KI) to remove the
steady state error which may slightly
deteriorate the transient performance.

PI compensator

= +

()
0

## The BASE response

Ess = 0
No overshoot

Feedforward compensator

## Feedforward compensator construction

Model the disturbance as function of Angles.
= =

From 90 to 35

From 35 to 90

Shoulder compensator

14.71%
31.08%

Trajectory Planning
The trajectory should respect:

Joints limits.
Max. velocities.
Max. accelerations.
Max effort (joint torques).

## to find a trajectory that connects an initial to a final configuration. we

consider planning the trajectory for a single joint.
q(t)
Where q(t) is a scalar joint variable
At time t0 :

At time tf:

follows:

## Cubic Polynomial Trajectories

Quantic Polynomial Trajectories
Linear Segment Method with Parabolic Blends or (LSPB)

## Cubic Polynomial Trajectories

Requires 4 constraints

## Smooth velocity and trajectory

Discontinuous acceleration

## Quantic Polynomial Trajectories

Requires 6 constraints

Smooth acceleration

## Linear Segment Method with Parabolic Blends or (LSPB)

constant velocity is desired along a portion of the path.

Ramp-up

Blend Region

Ramp-down

Initial configuration:
q(0) = q0

q(0) = 0
Final configuration:
q t f = qf
q tf = 0
Between 0 and tb (Ramp-up) we have :

## Between tf-tb and tf (Ramped-dwon) the path is found by symmetry,

symmetric to the Ramp Up with respect to (tf/2).
Thus the Complete LSPB:

LSPB trajectory q0= 0, qf = 40, and t0 =0, tf =1, Vmax = 60. In this
case tb=1/3 .

## End-effector generated path after passing by the waypoints from initial

position to final one

## ED-7220C motion planning

In this task; the robot EFF is sent from an
initial position to a goal position in the
presence of Obstacles taking the minimum
path.
Rapid-exploring
Random
algorithm is used.

Tree

(RRT)

1.
2.
3.
4.

## RRT state space: the manipulator workspace X.

Boundary values:
Collision Detector:
Input: the control or action () that can affect
the state.
5. Incremental Simulator: compute + for
+ .
6. Metric: determine the distance between two
pairs.

## Initialize search tree (T) with

initial position (Pinit ) of the
end-effector

## W h i l e tre e (T) < th e

m axi m u m s i z e of th e tre e
(Tm ax)

No

FAILURE

Yes

## Position sample (Psample)

from workspace
Look for the nearest
position (Pnearest ) in tree (T) to
Psample

Tree (RRT)
Flow Chart

## Employ a local planner to

find a motion from Pnearest to
the new position (Pnew)in the
direction of Psample

No

The motion is
collision-free
?
Yes

## add Pnew to tree (T) with an

edge from Pnearest to Pnew

Yes

No

Pnew is a goal
?

motion to Pnew

## RRT for ED 7220-C

Two collision detection are
taking in consideration:
End Effector.

3D exploration

## Robotic Operating System (ROS)

ROS is a set of software
libraries and tools (open
source) that help the
user to build robot
applications.

## Motion Planning using MoveIt and Rviz

packages.

Robot Modeling
Unified Robot Description Format (URDF).

Continuous.
Fixed.
Revolute.
Prismatic.

Position
constraints.

## The URDF is also used to define environment

with obstacles (Links + fixed joints).

## Rviz + MoveIt Motion Planning

Rviz; simulator integrated in ROS; used to
visualize the Robot and environment in 3D.
MoveIt ; OMPL package connected to Rviz;
allow the user to use different planning
algorithm between an initial and final arm
Poses.

ED 7220 C in Rviz

## MoveIt! Setup Assistant

Create ED 7220 C
MoveIt packages
form its URDF.

Home Pose

Pick pose

Place Pose

Planning algorithm

RRT configuration

Scene Obstacles

Final Scene

BASE Joint

## The end effector generated path (case2)

Waypoints
Waypoints are selected points in the resulting
path.
Cartesian waypoints [X Y Z].
Joint Space [ ].

Remarks:
To execute the motion planning in the real
robot we use the joint space waypoints (FK).
The waypoints are send serially from the PC
to the Arduino.
Between two waypoints; a time delay Td is set.
Td

Td is selected experimentally:
To get a smooth motion; where the controller has
to be updated before the joint ( ) settle at the
previous command.
Path achieved in reality has to have small error
with simulated robot motion planning.

Video

CONCLUSION
Study the Kinematic of ED 7220C:
Forward Kinematic.
Inverse Kinematic.

Design controllers:
Precision.