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.
