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.

Disadvantages
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.
deflection of the links under load, and vibration.

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

Design of Controllers
Cascade compensator:

Feedforward compensator.

Zero steady state error

( 0).

To reject and cancel the

disturbances introduced on
the Links:

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:

Trajectory planning methods require polynomial functions as

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 :

Applying the constraints:

Between tb and tf-tb (Blend region) we have:

Applying the constraints:

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 .

LSPB trajectory curve of the Base joint position

LSPB trajectory curve of the Base joint velocity

LSPB trajectory curves of the Base joint acceleration

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)

RRT (How It Works ?)

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

Rapid exploring Random

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
?

Return SUCCESS and the

motion to Pnew

RRT for ED 7220-C

Two collision detection are
taking in consideration:
End Effector.
Robot Links.

3D exploration

Trajectory generation and motion planning diagram

Robotic Operating System (ROS)

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

 Robot 3D modeling using URDF.

Motion Planning using MoveIt and Rviz

packages.

Robot Modeling
Unified Robot Description Format (URDF).

Continuous.
Fixed.
Revolute.
Prismatic.

Effort, Velocity and

Position
constraints.

 The URDF is also used to define environment

with obstacles (Links + fixed joints).

ED 7220C ROS Model

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

Case 1: pick and place without obstacle

BASE Joint

The end effector generated path (case1)

Case 2: pick and place with obstacle

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.
steady state error.
Cancel the disturbances and reduce the vibrations.

LSPB is used to generate the trajectory.

RRT algorithms is used to explore the
environment to avoid obstacles.