Motion Control of Industrial Robots in Operational Space Analysis and Experiments With the Pa10 Arm
Control of industrial robots Motion planning
Transcript of Control of industrial robots Motion planning
Control of industrial robots
Motion planning
Prof. Paolo Rocco ([email protected])Politecnico di Milano, Dipartimento di Elettronica, Informazione e Bioingegneria
Control of industrial robots โ Motion planning โ Paolo Rocco
With the motion planning we want to assign the way the robot evolves from an initial posture to a final one.
Motion planning is one of the essential problems in robotics. Most of the success on the market of a robot depends on the quality of motion planning.
We will address the following issues related to motion planning:โข introductory definitions (path vs. trajectory)โข motion programmingโข path planning with obstacle avoidanceโข trajectories in joint space (point-to-point and with intermediate points)โข kinematic and dynamic scaling of trajectoriesโข trajectories in Cartesian space (position and orientation trajectories)
Motion planning
Control of industrial robots โ Motion planning โ Paolo Rocco
We need to introduce some terminology concerning concepts which are often confused:
Path: it is a geometric concept and stands for a line in a certain space (the space of Cartesian positions, the space of the orientations, the joint space,..) to be followed by the object whose motion has to be planned
Timing law: it is the time dependence with which we want the robot to travel along the assigned path
Trajectory: it is a path over which a timing law has been assigned
The final result of a motion planning problem is thus a trajectory that will then serve as an input to the real-time position/velocity controllers.
Definitions
Control of industrial robots โ Motion planning โ Paolo Rocco
Trajectories in the operational space: the path (position and orientation) of the robot end effector is specified in the common Cartesian space
task description is natural constraints on the path can be accounted for singular points or redundant degrees of
freedom generate problems online kinematic inversion is needed
Trajectories in the operational space
Control of industrial robots โ Motion planning โ Paolo Rocco
Trajectories in joint space: the desired joint positions are directly specified
problems related to kinematic singularities and redundant degrees of freedom are solved directly
it is a mode of interest when we just want that the axes move from an initial to a final pose (and we are not interested in the resulting motion of the end effector)
online kinematic inversion is not needed
Trajectories in joint space
Control of industrial robots โ Motion planning โ Paolo Rocco
InverseKinematicstarget
stateINSTRUCTION
STACK
TrajGen
Axiscontroller
state update
Instruction stack: list of instructions to be executed, specified using the proprietary programming language
Trajectory generation: converts an instruction into a trajectory to be executed Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed) Axis controllers & drives: closes the control loop ensuring tracking performance
Elements of a motion planning and control system
Control of industrial robots โ Motion planning โ Paolo Rocco
InverseKinematicstarget
stateINSTRUCTION
STACK
TrajGen
Axiscontroller
state update
Instruction stack: list of instructions to be executed, specified using the proprietary programming language
Trajectory generation: converts an instruction into a trajectory to be executed Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed) Axis controllers & drives: closes the control loop ensuring tracking performance
Elements of a motion planning and control system
Control of industrial robots โ Motion planning โ Paolo Rocco
A first mode for motion programming is the so called teaching-by-showing (also known as lead-through programming).
Using the teach-pendant, the operator moves the manipulator along the desired path. Position transducers memorize the positions the robot has to reach, which will be then jointed by a software for trajectory generation, possibly using some of the intermediate points as via points.The robot will be then able to autonomously repeat the motion.
COMAU SpA
No particular programming skills are requested to the operator. On the other hand the method has limitations, since making a program requires that the programmer has the robot at his/her disposal (and then the robot is not productive), there is no possibility to execute logical functions or wait cycles and in general it is not possible to program complex activities.
Teaching by showing
Control of industrial robots โ Motion planning โ Paolo Rocco
The generation of the robot program can also be done in a robot programming environment. The robot programmer can move the robot in a virtual environment with high fidelity rendering of the robot motion in the robotic cell.
There are tools to record positions and to make the robot move along a path formed by such positions.
At the end the robot programming environment produces the code ready to be downloaded into the robot controller.
ABB RobotStudio
Programming environments
Control of industrial robots โ Motion planning โ Paolo Rocco
The robot programmer can also write the robot program directly using a robotic programming language.
With a robotic programming language the operator can program the motion of the robot as well as complex operations where the robot, inside a work-cell, interacts with other machines and devices. With respect to a general purpose programming language, the language provides specific robot oriented functionalities.
In the following, we will discuss some elements of the PDL2 programming language by COMAU Robotics.
Programming languages
Control of industrial robots โ Motion planning โ Paolo Rocco
The program moves pieces from a feeder to a table or to a discard bin, depending on digital input signals:
PROGRAM packVAR
home, feeder, table, discard : POSITIONBEGIN CYCLE
MOVE TO homeOPEN HAND 1WAIT FOR $DIN[1] = ON
-- signals feeder readyMOVE TO feederCLOSE HAND 1IF $DIN[2] = OFF THEN
-- determines if good partMOVE TO table
ELSEMOVE TO discard
ENDIFOPEN HAND 1
-- drop part on table or in binEND pack
1. Feeder2. Robot3. Discard Bin4. Table
Program example
Control of industrial robots โ Motion planning โ Paolo Rocco
Besides data types typical of any programming language (integer, real, boolean, string, array), some types specific to robotic applications are defined in PDL2. Among them:
VECTOR: representation of a vector through three components
POSITION: three components of Cartesian position, three orientation components (Euler angles) and a configuration string (which indicates whether it is a shoulder/elbow/wrist upper/lower configuration)
JOINTPOS: positions of the joints, measured in degrees
Data types
Control of industrial robots โ Motion planning โ Paolo Rocco
In PDL2 for each manipulator a world reference frame is defined. The operator might redefine the base frame ($BASE) relative to the world frame. This is useful when the robot has to be repositioned in the work area, since it avoids re-computation of all the positions of objects in the cell.
Reference frames
Similarly the programmer can define a frame ($TOOL) relative to the tool frame of the manipulator, which is useful whenever the tool mounted on the end effector is changed.
Control of industrial robots โ Motion planning โ Paolo Rocco
With the instruction MOVE, motion commands of the arms are given. The format of the instruction is as follows:
MOVE <ARM[n]> <trajectory> dest_clause <opt_clauses> <sync_clause>
(note that a single controller can manage several arms).
The trajectory clause can take one of the following values:
LINEARCIRCULARJOINT
(linear motion in Cartesian space)(circular motion in Cartesian space)(motion in joint space)
The default is a motion in joint space.
The instruction MOVE
Control of industrial robots โ Motion planning โ Paolo Rocco
There are several destination clauses for the instruction MOVE. Main ones are:
MOVE TOIt moves the arm towards the specified destination, which might be a variable of type POSITION or JOINTPOS. For example:
MOVE LINEAR TO POS(x,y,z,e1,e2,e3,config)MOVE TO home
The optional VIA clause can be used with the MOVE TO destination clause to specify a position through which the arm passes between the initial position and the destination. The VIA clause is used most commonly to define an arc for circular moves. For example:
MOVE TO initialMOVE CIRCULAR TO destination VIA arc
MOVE: destination clauses
Control of industrial robots โ Motion planning โ Paolo Rocco
MOVE NEARWith this clause it is possible to specify a destination which is located along the approach vector of the tool, within a certain distance (expressed in mm) from a certain position. Example:
MOVE NEAR destination BY 250.0
MOVE AWAYA destination can be specified along the approach vector of the tool, at a specified distance from the current position. Example:MOVE AWAY 250.0
MOVE: destination clauses
Control of industrial robots โ Motion planning โ Paolo Rocco
MOVE RELATIVEA destination relative to the current position of the frame can be specified. Example:MOVE RELATIVE VEC(100,0,100) IN frame
MOVE ABOUTIt defines the destination that the tool has to reach after a rotation around the specified vector, with respect to the current position. Example:
MOVE ABOUT VEC(0,100,0) BY 90 IN frame
frame can be either TOOL or BASE
MOVE: destination clauses
Control of industrial robots โ Motion planning โ Paolo Rocco
MOVE BYAllows the programmer to specify a destination as a list of REAL expressions, with each item corresponding to an incremental move for the joint of an arm. Example:
MOVE BY {alpha, beta, gamma, delta, omega}
MOVE FORAllows the programmer to specify a partial move along the trajectory towards a theoretical destination. The orientation of the tool changes in proportion to the distance. Example:
MOVE: destination clauses
Control of industrial robots โ Motion planning โ Paolo Rocco
Using the optional clause WITH it is possible to assign values to some predefined temporary variables. In particular we can operate over the following variables:
$PROG_SPD_OVRIt is a percentage by which we can modify the default speed value used by the controller for joint space motions.
$LIN_SPD
It is the value of linear speed for a Cartesian motion, expressed in m/s:
$PROG_ACC_OVR, $PROG_DEC_OVR
They are percentages by which we can modify the default values of acceleration and deceleration used by the controller for joint space motions.
Examples: MOVE TO p1 WITH $PROG_SPD_OVR=50MOVE LINEAR TO p2 WITH $LIN_SPD=0.6
Setting the speed and the acceleration
Control of industrial robots โ Motion planning โ Paolo Rocco
If we use the instruction MOVEFLY and the motion is followed by another motion, the arm will not stop at the first destination, but will move from the starting point of the first motion to the end point of the second motion, without stopping in the common point of the two motions. Example:
MOVE TO aMOVEFLY TO b ADVANCEMOVE TO c
(the ADVANCE clause allows the interpretation of the next MOVE instruction as soon as the first motion begins)
Continuous motion (MOVEFLY)
Control of industrial robots โ Motion planning โ Paolo Rocco
If the robot moves in a cluttered environment, the definition of the path might be troublesome In fact we need a path which is collision free: such path, at the current stage of robotics practice, is
generated manually by the programmer as a sequence of MOVE commands
An active research line consists in automatic path planning, i.e. in finding an algorithm that autonomously generate the geometric path, given the kinematics of the robot and the known positions and shapes of the obstacles
Path planning with obstacle avoidance
Control of industrial robots โ Motion planning โ Paolo Rocco
Usually the path planning problem is addressed in the configuration space (C-space), where the robot is at each time instant represented as a mobile point
For an articulated robot, the common choice of the configuration space is the space of the joint variables (joint space)
Obstacles are mapped from the workspace to the configuration space: in case of an articulated manipulator they take complicated shapes
Workspace C-space
We call Cfree the subset of C-space that does not cause collisions with the obstacles.A path in C-space is free if it is entirely contained in Cfree
Configuration space
Control of industrial robots โ Motion planning โ Paolo Rocco
The path planning problem can be set as follows in general:
WorkspaceC-space
Assume that the initial and final posture of the robot in the Workspace are mapped into corresponding configurations in C-space, respectively a start configuration ๐ช๐ชS and a goal configuration ๐ช๐ชG.
Planning a collision-free motion for the robot means to generate a path from ๐ช๐ชS to ๐ช๐ชG if they belong to the same connected component of Cfree, and to report a failure otherwise.
Setting the problem
Control of industrial robots โ Motion planning โ Paolo Rocco
The exact planning algorithms that are based on the a priori knowledge of the complete C-space are exponential in the dimensionality of the C-space and thus hardly applicable in practice
Probabilistic planners are instead a class of methods of remarkable efficiency, especially for high-dimension configuration spaces
They belong to the general family of sampling-based methods, where the basic idea is to randomlyselect a finite-set of collision-free configurations that adequately represent how Cfree is connected, and using these configurations to build a roadmap
At each iteration a sample configuration is chosen and it is checked whether it entails a collision between the robot and the obstacles: if yes, the configuration is discarded, otherwise it is added to the current roadmap
Two versions of such randomized sampling-based approach are: PRM (Probabilistic Roadmap) RRT (Rapidly-exploring Random Tree)
Probabilistic planning
Control of industrial robots โ Motion planning โ Paolo Rocco
a random sample ๐ช๐ชrand of the C-space is selected using a uniform probability distribution and tested for collision
if ๐ช๐ชrand does not cause collisions it is added to a roadmap which is progressively being formed and connected (if possible) through free local paths to sufficiently โnearโ configurations already in the roadmap
the generation of a free local path between ๐ช๐ชrand and a near configuration ๐ช๐ชnear is made by a procedure called local planner
the iterations terminate when either a maximum number of iterations has been reached or the number of connected components in the roadmap becomes smaller than a given threshold
then we try to verify whether the path panning problem can be satisfied by connecting ๐ช๐ชSand ๐ช๐ชG to the same connected component of the PRM by free local paths
PRM (Probabilistic roadmap)
Control of industrial robots โ Motion planning โ Paolo Rocco
This picture is taken from the textbook:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009
PRM (Probabilistic roadmap)
Control of industrial robots โ Motion planning โ Paolo Rocco
Advantages of PRM: it is remarkably fast in finding solutions to motion planning problems unlike other methods, there is no need to generate the obstacles in C-space
Disadvantages of PRM: it is only probabilistically complete: the probability to find a solution to the planning
problem, if it exists, tends to 1 as the execution time tends to infinity
PRM (Probabilistic roadmap)
Control of industrial robots โ Motion planning โ Paolo Rocco
the method makes use of a data structure called tree a random sample ๐ช๐ชrand of the C-space is selected using a uniform probability distribution the configuration ๐ช๐ชnear in the tree T (which is progressively formed) that is the closest one to ๐ช๐ชrand is found and a new candidate configuration ๐ช๐ชnew is produced on the segment joining ๐ช๐ชnear to ๐ช๐ชrand at a predefined distance ฮด from ๐ช๐ชnear
it is checked that both ๐ช๐ชnew and the segment joining ๐ช๐ชnear to ๐ช๐ชnew belong to Cfree
if this is true, the tree T is expanded by incorporating ๐ช๐ชnew and the said segment to speed up the search, two trees are expanded, rooted at ๐ช๐ชS and ๐ช๐ชG, respectively at each iteration, both trees are expanded with the randomized procedure after a certain number of expansion steps, the algorithm tries to connect the two trees and
thus to complete the path
RRT (Rapidly-exploring random trees)
Control of industrial robots โ Motion planning โ Paolo Rocco
This picture is taken from the textbook:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009
RRT (Rapidly-exploring random trees)
Control of industrial robots โ Motion planning โ Paolo Rocco
the RRT expansion technique, though simple, results in a very efficient exploration of the C-space, since the procedure for generating new candidate configurations is intrinsically biased towards regions in Cfree that have not been visited yet
as in the PRM method, there is no need to generate the obstacles in C-space like the PRM method, it is only probabilistically complete
Example of path generated with a RRT algorithm
RRT (Rapidly-exploring random trees)
Control of industrial robots โ Motion planning โ Paolo Rocco
Ample free space Cluttered space Narrow passages
RRT (Rapidly-exploring random trees)
Control of industrial robots โ Motion planning โ Paolo Rocco
RRT (Rapidly-exploring random trees)
Control of industrial robots โ Motion planning โ Paolo Rocco
in the artificial potential method, the motion of the point that represents the robot in C-space is influenced by a potential field ๐๐
this field is obtained as the superposition of an attractive potential to the goal and a repulsive potential from the obstacles.
at each configuration ๐ช๐ช the artificial force generated by the potential is defined as the negative gradient โโ๐๐ ๐ช๐ช of the potential
paraboloid potential conic potential
The method is particularly used in online path planning
This picture is taken from the textbook:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009
local minima!
Artificial potentials
Control of industrial robots โ Motion planning โ Paolo Rocco
InverseKinematicstarget
stateINSTRUCTION
STACK
TrajGen
Axiscontroller
state update
Instruction stack: list of instructions to be executed, specified using the proprietary programming language
Trajectory generation: converts an instruction into a trajectory to be executed Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed) Axis controllers & drives: closes the control loop ensuring tracking performance
Elements of a motion planning and control system
Control of industrial robots โ Motion planning โ Paolo Rocco
As we have seen, in general the user, when assigning a motion command, specifies a restricted number of parameters as inputs:
Space of definition: joint space or Cartesian space
For the path: endpoints, possible intermediate points, path geometry (segment, circular arc, etc..)
For the timing law: overall travelling time, maximum velocity and/or acceleration (or percentages thereof)
Based on this information the trajectory planner generates a dense sequence of intermediate points in the relevant space (joint space or Cartesian space) at a fixed rate (e.g. 10 ms).In case of Cartesian space trajectories these points are additionally converted into points in joint space through kinematic inversion.These values can the be interpolated in order to match the rate of the motion controller (e.g 1ms or 500ยตs): this is also called micro-interpolation
Inputs and outputs of a trajectory planner
Control of industrial robots โ Motion planning โ Paolo Rocco
Some criteria for the selection of the trajectories might be:
computational efficiency and memory space
continuity of positions, velocities (and possibly of accelerations and jerks)
minimization of undesired effects (oscillations, irregular curvature)
accuracy (no overshoot on final position)
Criteria for trajectory selection
Control of industrial robots โ Motion planning โ Paolo Rocco
When we plan the trajectory in joint space we want to generate a function ๐ช๐ช ๐ก๐ก which interpolates the values assigned for the joint variables at the initial and final points.It is sufficient to work component-wise (i.e. we consider a single joint variable ๐๐๐๐ ๐ก๐ก ): in the following we will then consider planning of a scalar variable.
Two situations are of interest:
Point-to-point motion:we just specify the endpoints
Interpolation of points:we also specify some intermediate points
When planning in joint space, the definition of the path as a geometric entity is not an issue, since we are not interested in a coordinated motion of the joints (apart from having all the joints complete their motion at the same instant).
Trajectories in joint space
Control of industrial robots โ Motion planning โ Paolo Rocco
The simplest case of trajectory planning for point-to-point motion is when some initial and final conditions are assigned on positions, velocities and possibly on acceleration and jerk and the travel time.
Polynomial functions of the following kind can be considered:
๐๐ ๐ก๐ก = ๐๐0 + ๐๐1๐ก๐ก + ๐๐2๐ก๐ก2 + โฏ+ ๐๐๐๐๐ก๐ก๐๐
The higher the degree ๐๐ of the polynomial, the larger the number of boundary conditions that can be satisfied and the smoother the trajectory will be.
t
q
ti
qi
?tf
qf
Polynomial trajectories
Control of industrial robots โ Motion planning โ Paolo Rocco
Suppose that the following boundary conditions are assigned: an initial and a final instants ๐ก๐ก๐๐ and ๐ก๐ก๐๐ initial position and velocity ๐๐๐๐ and ๏ฟฝฬ๏ฟฝ๐๐๐ final position and velocity ๐๐๐๐ and ๏ฟฝฬ๏ฟฝ๐๐๐
We then have four boundary conditions. In order to satisfy them we need a polynomial of order at least equal to three (cubic polynomial):
If we impose the boundary conditions:
๐๐ ๐ก๐ก๐๐ = ๐๐๐๐๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐๐๐ ๐ก๐ก๐๐ = ๐๐๐๐๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐
๐๐ ๐ก๐ก = ๐๐0 + ๐๐1 ๐ก๐ก โ ๐ก๐ก๐๐ + ๐๐2 ๐ก๐ก โ ๐ก๐ก๐๐ 2 + ๐๐3 ๐ก๐ก โ ๐ก๐ก๐๐ 3
we obtain:
๐๐0 = ๐๐๐๐๐๐1 = ๏ฟฝฬ๏ฟฝ๐๐๐
๐๐2 =โ3 ๐๐๐๐ โ ๐๐๐๐ โ 2๏ฟฝฬ๏ฟฝ๐๐๐ + ๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐
๐๐2
๐๐3 =2 ๐๐๐๐ โ ๐๐๐๐ + ๏ฟฝฬ๏ฟฝ๐๐๐ + ๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐
๐๐3 ๐๐ = ๐ก๐ก๐๐ โ ๐ก๐ก๐๐
Cubic polynomials
Control of industrial robots โ Motion planning โ Paolo Rocco
๐ก๐ก๐๐ = 0, ๐ก๐ก๐๐ = 1 ๐ ๐ ,
๐๐๐๐ = 10ยฐ, ๐๐๐๐ = 30ยฐ,
๏ฟฝฬ๏ฟฝ๐๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐ = 0ยฐ/๐ ๐
-0.2 0 0.2 0.4 0.6 0.8 10
10
20
30
40
deg
Position
-0.2 0 0.2 0.4 0.6 0.8 1-10
0
10
20
30
40
t(s)
deg/
s
Velocity
-0.2 0 0.2 0.4 0.6 0.8 1-150
-100
-50
0
50
100
150
t(s)
deg/
s2
Acceleration
Cubic polynomials: example
Control of industrial robots โ Motion planning โ Paolo Rocco
In order to assign conditions also on the accelerations, we need to consider polynomials of degree 5:
๐๐ ๐ก๐ก = ๐๐0 + ๐๐1 ๐ก๐ก โ ๐ก๐ก๐๐ + ๐๐2 ๐ก๐ก โ ๐ก๐ก๐๐ 2 + ๐๐3 ๐ก๐ก โ ๐ก๐ก๐๐ 3 + ๐๐4 ๐ก๐ก โ ๐ก๐ก๐๐ 4 + ๐๐5 ๐ก๐ก โ ๐ก๐ก๐๐ 5
Imposing boundary conditions:
๐๐ ๐ก๐ก๐๐ = ๐๐๐๐ ๐๐ ๐ก๐ก๐๐ = ๐๐๐๐๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐ ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐ ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐
we obtain:
๐๐0 = ๐๐๐๐๐๐1 = ๏ฟฝฬ๏ฟฝ๐๐๐๐๐2 =
12 ๏ฟฝฬ๏ฟฝ๐๐๐
๐๐3 =20 ๐๐๐๐ โ ๐๐๐๐ โ 8๏ฟฝฬ๏ฟฝ๐๐๐ + 12๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐ โ 3๏ฟฝฬ๏ฟฝ๐๐๐ โ ๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐2
2๐๐3
๐๐4 =30 ๐๐๐๐ โ ๐๐๐๐ + 14๏ฟฝฬ๏ฟฝ๐๐๐ + 16๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐ + 3๏ฟฝฬ๏ฟฝ๐๐๐ โ 2๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐2
2๐๐4
๐๐5 =12 ๐๐๐๐ โ ๐๐๐๐ โ 6 ๏ฟฝฬ๏ฟฝ๐๐๐ + ๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐ โ ๏ฟฝฬ๏ฟฝ๐๐๐ โ ๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐2
2๐๐5 ๐๐ = ๐ก๐ก๐๐ โ ๐ก๐ก๐๐
Polynomials of degree five
Control of industrial robots โ Motion planning โ Paolo Rocco
๐ก๐ก๐๐ = 0, ๐ก๐ก๐๐ = 1 ๐ ๐ ,
๐๐๐๐ = 10ยฐ, ๐๐๐๐ = 30ยฐ,
๏ฟฝฬ๏ฟฝ๐๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐ = 0,
๏ฟฝฬ๏ฟฝ๐๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐ = 0
-0.2 0 0.2 0.4 0.6 0.8 10
10
20
30
40
deg
Position
-0.2 0 0.2 0.4 0.6 0.8 1-10
0
10
20
30
40
t(s)
deg/
s
Velocity
-0.2 0 0.2 0.4 0.6 0.8 1-150
-100
-50
0
50
100
150
t(s)
deg/
s2Acceleration
Polynomials of degree five: example
Control of industrial robots โ Motion planning โ Paolo Rocco
The harmonic trajectory generalizes the equation of a harmonic motion, where the acceleration is proportional to the position, with opposite sign. A harmonic trajectory has continuous derivatives of all orders in all the internal points of the trajectory.
The equations are:
๐๐ ๐ก๐ก =๐๐๐๐ โ ๐๐๐๐
2 1 โ cos๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
+ ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๐๐ ๐๐๐๐ โ ๐๐๐๐2 ๐ก๐ก๐๐ โ ๐ก๐ก๐๐
sin๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๐๐2 ๐๐๐๐ โ ๐๐๐๐2 ๐ก๐ก๐๐ โ ๐ก๐ก๐๐
2 cos๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
๐๐ ๐ก๐ก๐๐ = ๐๐๐๐ , ๐๐ ๐ก๐ก๐๐ = ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0, ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0
Harmonic trajectory
Control of industrial robots โ Motion planning โ Paolo Rocco
๐ก๐ก๐๐ = 0, ๐ก๐ก๐๐ = 1 ๐ ๐ ,
๐๐๐๐ = 10ยฐ, ๐๐๐๐ = 30ยฐ
-0.2 0 0.2 0.4 0.6 0.8 10
10
20
30
40
deg
Position
-0.2 0 0.2 0.4 0.6 0.8 1-10
0
10
20
30
40
t(s)
deg/
s
Velocity
-0.2 0 0.2 0.4 0.6 0.8 1-150
-100
-50
0
50
100
150
t(s)
deg/
s2Acceleration
Harmonic trajectory (example)
Control of industrial robots โ Motion planning โ Paolo Rocco
The harmonic trajectory has discontinuities in the acceleration in the initial and final instants, and then undefined (or infinite) values of jerk. An alternative is the cycloidal trajectory, which is continuous in the acceleration, too.
Here are the equations:
๐๐ ๐ก๐ก = ๐๐๐๐ โ ๐๐๐๐๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
โ12๐๐ sin
2๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
+ ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๐๐๐๐ โ ๐๐๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
1 โ cos2๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =2๐๐ ๐๐๐๐ โ ๐๐๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
2 sin2๐๐ ๐ก๐ก โ ๐ก๐ก๐๐๐ก๐ก๐๐ โ ๐ก๐ก๐๐
๐๐ ๐ก๐ก๐๐ = ๐๐๐๐ , ๐๐ ๐ก๐ก๐๐ = ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0, ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0, ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐ = 0
Cycloidal trajectory
Control of industrial robots โ Motion planning โ Paolo Rocco
๐ก๐ก๐๐ = 0, ๐ก๐ก๐๐ = 1 ๐ ๐ ,
๐๐๐๐ = 10ยฐ, ๐๐๐๐ = 30ยฐ
-0.2 0 0.2 0.4 0.6 0.8 10
10
20
30
40
deg
Position
-0.2 0 0.2 0.4 0.6 0.8 1-10
0
10
20
30
40
t(s)
deg/
s
Velocity
-0.2 0 0.2 0.4 0.6 0.8 1-150
-100
-50
0
50
100
150
t(s)
deg/
s2
Acceleration
Cycloidal trajectory (example)
Control of industrial robots โ Motion planning โ Paolo Rocco
A quite common industrial practice to generate the trajectory consists in planning a linear position profile adjusted at the beginning and at the end of the trajectory with parabolic bends. The resulting velocity profile has the typical trapezoidal shape.
The trajectory is then composed of three parts:
1. Constant accel., linear velocity, parabolic position;2. Zero acceleration, constant velocity, linear position;3. Constant deceleration, linear velocity, parabolic position.
Often the duration ๐๐๐๐ of the acceleration phase (phase 1) is set equal to the duration of the deceleration phase (phase 3): this way a trajectory is obtained, which is symmetric with respect to the central time instant. Of course it has to be ๐๐๐๐ โค โ๐ก๐ก๐๐ โ ๐ก๐ก๐๐ 2 .
tti+Ta
qv.q.
tfโTa tfti
1 2 3
Trapezoidal velocity profile (TVP)
Control of industrial robots โ Motion planning โ Paolo Rocco
Acceleration: Constant velocity:
Deceleration:
๐ก๐ก โ ๐ก๐ก๐๐ , ๐ก๐ก๐๐ + ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ๐๐๐๐
๐ก๐ก โ ๐ก๐ก๐๐
๐๐ ๐ก๐ก = ๐๐๐๐ +๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ
2๐๐๐๐๐ก๐ก โ ๐ก๐ก๐๐ 2
๐ก๐ก โ ๐ก๐ก๐๐ + ๐๐๐๐ , ๐ก๐ก๐๐ โ ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก = 0๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก = ๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ
๐๐ ๐ก๐ก = ๐๐๐๐ + ๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ ๐ก๐ก โ ๐ก๐ก๐๐ โ๐๐๐๐2
๐ก๐ก โ ๐ก๐ก๐๐ โ ๐๐๐๐ , ๐ก๐ก๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก = โ๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก =๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ๐๐๐๐
๐ก๐ก๐๐ โ ๐ก๐ก
๐๐ ๐ก๐ก = ๐๐๐๐ โ๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ
2๐๐๐๐๐ก๐ก๐๐ โ ๐ก๐ก 2
TVP: equations
Control of industrial robots โ Motion planning โ Paolo Rocco
๐ก๐ก๐๐ = 0, ๐ก๐ก๐๐ = 4๐ ๐ , ๐๐๐๐ = 1๐ ๐ ,
๐๐๐๐ = 0ยฐ, ๐๐๐๐ = 30ยฐ, ๏ฟฝฬ๏ฟฝ๐๐ฃ๐ฃ = 10ยฐ/๐ ๐
-1 0 1 2 3 4 50
10
20
30
deg
Position
-1 0 1 2 3 4 5
0
5
10
t(s)
deg/
s
Velocity
-1 0 1 2 3 4 5
-10
-5
0
5
10
t(s)
deg/
s2Acceleration
TVP: example
Control of industrial robots โ Motion planning โ Paolo Rocco
Once a trajectory has been planned, it is often necessary to scale it in order to satisfy the constraints on the actuation system, which emerge in terms of saturations.In particular we will consider:
Kinematic scaling of the trajectory is relevant for those trajectory profiles (cubic, harmonic, โฆ) for which such values are not assigned in the planning itself.
For kinematic scaling we can proceed joint by joint (no coupling effects) For dynamic scaling we will need to consider the whole coupled model of the robot
1. Kinematic scaling: the trajectory needs satisfy the constraints on the maximum velocity and acceleration;
2. Dynamic scaling: the trajectory needs satisfy the constraints on the maximum achievable torques/forces by the actuators.
Trajectory scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
In order to kinematically scale the trajectory it is convenient to express it in a parametric form, as a function of a suitably normalized parameter ๐๐ = ๐๐ ๐ก๐ก .Given the trajectory ๐๐ ๐ก๐ก , defined between points ๐๐๐๐ and ๐๐๐๐ with a travel time of ๐๐ = ๐ก๐ก๐๐ โ ๐ก๐ก๐๐ , its expression in normalized form is as follows:
๐๐ ๐ก๐ก = ๐๐๐๐ + โ๐๐ ๐๐ with โ = ๐๐๐๐ โ ๐๐๐๐ , 0 โค ๐๐ ๐๐ โค 1, ๐๐ = ๐ก๐กโ๐ก๐ก๐๐๐๐
, 0 โค ๐๐ โค 1
It follows:Maximum values of velocity, acceleration, etc., are obtained in correspondence to the maximum values of functions ๐๐ ๐๐ ๐๐ : by modifying the travel time ๐ป๐ป of the trajectory it is possible to satisfy the constraints on the kinematic saturations.
(normalized time)
Trajectory normalization
๐๐๐๐ ๐ก๐ก๐๐๐ก๐ก =
โ๐๐ ๐๐
โฒ ๐๐
๐๐2๐๐ ๐ก๐ก๐๐๐ก๐ก2 =
โ๐๐2 ๐๐
โฒโฒ ๐๐โฎ
๐๐๐๐๐๐ ๐ก๐ก๐๐๐ก๐ก๐๐ =
โ๐๐๐๐ ๐๐
๐๐ ๐๐
Control of industrial robots โ Motion planning โ Paolo Rocco
This trajectory can be parameterized with the polynomial:
๐๐ ๐๐ = ๐๐0 + ๐๐1๐๐ + ๐๐2๐๐2 + ๐๐3๐๐3
from which:
Assigning the boundary conditions ๐๐โฒ 0 = 0,๐๐โฒ 1 = 0 (besides ๐๐ 0 = 0,๐๐ 1 = 1):
๐๐0 = 0, ๐๐1 = 0, ๐๐2 = 3, ๐๐3 = โ2
๐๐ ๐๐ = 3๐๐2 โ 2๐๐3 ๐๐โณ ๐๐ = 6 โ 12๐๐๐๐โฒ ๐๐ = 6๐๐ โ 6๐๐2 ๐๐โด ๐๐ = โ12
Maximum values of velocity and acceleration are then:
๐๐โฒmax = ๐๐โฒ 0.5 =32
โ ๏ฟฝฬ๏ฟฝ๐max =3โ2๐๐
๐๐โณmax = ๐๐โณ 0 = 6 โ ๏ฟฝฬ๏ฟฝ๐max =6โ๐๐2
Polynomial trajectory of degree 3
Control of industrial robots โ Motion planning โ Paolo Rocco
๐๐ ๐๐ = ๐๐0 + ๐๐1๐๐ + ๐๐2๐๐2 + ๐๐3๐๐3 + ๐๐4๐๐4 + ๐๐5๐๐5
from which:
Assigning boundary conditions ๐๐ 0 = 0,๐๐ 1 = 1, ๐๐โฒ 0 = 0,๐๐โฒ 1 = 0, ๐๐โณ 0 = 0,๐๐โณ 1 = 0
๐๐0 = 0, ๐๐1 = 0, ๐๐2 = 0, ๐๐3 = 10, ๐๐4 = โ15, ๐๐5 = 6
๐๐ ๐๐ = 10๐๐3 โ 15๐๐4 + 6๐๐5 ๐๐โณ ๐๐ = 60๐๐ โ 180๐๐2 + 120๐๐3
๐๐โฒ ๐๐ = 30๐๐2 โ 60๐๐3 + 30๐๐4 ๐๐โด ๐๐ = 60 โ 360๐๐ + 360๐๐2
๐๐โฒmax = ๐๐โฒ 0.5 =158
โ ๏ฟฝฬ๏ฟฝ๐max =15โ8๐๐
๐๐โณmax = ๐๐โณ 0.2123 =10 3
3โ ๏ฟฝฬ๏ฟฝ๐max =
10 3โ3๐๐2
This trajectory can be parameterized with the polynomial:
Maximum values of velocity and acceleration are then:
Polynomial trajectory of degree 5
Control of industrial robots โ Motion planning โ Paolo Rocco
๐๐ ๐๐ =12
1 โ cos ๐๐๐๐
from which: ๐๐โฒ ๐๐ =๐๐2
sin ๐๐๐๐
๐๐โณ ๐๐ =๐๐2
2cos ๐๐๐๐
๐๐โด ๐๐ =๐๐3
2sin ๐๐๐๐
๐๐โฒmax = ๐๐โฒ 0.5 =๐๐2
โ ๏ฟฝฬ๏ฟฝ๐max =๐๐โ2๐๐
๐๐โณmax = ๐๐โณ 0 =๐๐2
2โ ๏ฟฝฬ๏ฟฝ๐max =
๐๐2โ2๐๐2
This trajectory can be parameterized with the function:
Maximum values of velocity and acceleration are then:
Harmonic trajectory
Control of industrial robots โ Motion planning โ Paolo Rocco
๐๐ ๐๐ = ๐๐ โ12๐๐
sin 2๐๐๐๐from which:
๐๐โฒ ๐๐ = 1 โ cos 2๐๐๐๐๐๐โณ ๐๐ = 2๐๐ sin 2๐๐๐๐๐๐โด ๐๐ = 4๐๐2 cos 2๐๐๐๐
๐๐โฒmax = ๐๐โฒ 0.5 = 2 โ ๏ฟฝฬ๏ฟฝ๐max = 2โ๐๐
๐๐โณmax = ๐๐โณ 0.25 = 2๐๐ โ ๏ฟฝฬ๏ฟฝ๐max = 2๐๐โ๐๐2
This trajectory can be parameterized with the function:
Maximum values of velocity and acceleration are then:
Cycloidal trajectory
Control of industrial robots โ Motion planning โ Paolo Rocco
We want to design a trajectory with ๐๐๐๐ = 10ยฐ, ๐๐๐๐ = 50ยฐ,
for an actuator characterized by ๏ฟฝฬ๏ฟฝ๐๐๐๐๐๐๐ = 30 โยฐ ๐ ๐ , ๏ฟฝฬ๏ฟฝ๐๐๐๐๐๐๐ = 80 โยฐ ๐ ๐ 2.
The following results can be obtained (โ = 40ยฐ):
667.2772.1
802
667.2302
2
2Cycloidal
094.2571.1
160
094.260
2
2Harmonic
5.2699.1
240310
5.224015
3310
815
degree 5th Polin.
2732.1
806
2603
623
degree 3rd Polin.
sConstraintFormulasTrajectory
2max
max
2
2
2
max
max
2max
max
2max
max
min
=ฯ
=
==
ฯ=
=
=ฯ
=
=ฯ
=
ฯ=
ฯ=
==
==
=
=
==
==
=
=
hT
hT
Thq
Thq
hT
hT
Thq
Thq
hT
hT
Thq
Thq
hT
hT
ThqThq
T
fast
fast
Kinematic scaling: an example
Control of industrial robots โ Motion planning โ Paolo Rocco
We will discuss the dynamic scaling technique making reference directly to the model of the robotic manipulator (neglecting friction effects):
๐๐ ๐ช๐ช ๏ฟฝฬ๏ฟฝ๐ช + ๐๐ ๐ช๐ช, ๏ฟฝฬ๏ฟฝ๐ช ๏ฟฝฬ๏ฟฝ๐ช + ๐ ๐ ๐ช๐ช = ฯ
For each joint an equation of the following kind holds:
๐๐๐๐๐๐ ๐ช๐ช ๏ฟฝฬ๏ฟฝ๐ช +12๏ฟฝฬ๏ฟฝ๐ช๐๐๐๐๐๐ ๐ช๐ช ๏ฟฝฬ๏ฟฝ๐ช + ๐๐๐๐ ๐ช๐ช = ๐๐๐๐ ๐๐ = 1, โฆ ,๐๐
where ๐๐๐๐ ๐ช๐ช is a suitable matrix.
Let us consider a parameterization of the trajectory in terms of a scalar function:
๐ช๐ช = ๐ช๐ช ๐๐ ,๐๐ = ๐๐ ๐ก๐ก
Then: ๏ฟฝฬ๏ฟฝ๐ช =๐๐๐ช๐ช๐๐๐๐
๏ฟฝฬ๏ฟฝ๐, ๏ฟฝฬ๏ฟฝ๐ช =๐๐2๐ช๐ช๐๐๐๐2
๏ฟฝฬ๏ฟฝ๐2 +๐๐๐ช๐ช๐๐๐๐
๏ฟฝฬ๏ฟฝ๐
(this means that all the joint positions depend on the time in the same way)
Dynamic scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
By substituting in the dynamic equation of the ๐๐๐ก๐ก๐ก joint we have:
i.e. an equation in the form:
๐๐๐๐๐๐ ๐ช๐ช ๐๐๐๐๐ช๐ช๐๐๐๐
๏ฟฝฬ๏ฟฝ๐ + ๐๐๐๐๐๐ ๐ช๐ช ๐๐๐๐2๐ช๐ช๐๐๐๐2
+12๐๐๐ช๐ช๐๐
๐๐๐๐๐๐๐๐ ๐ช๐ช ๐๐
๐๐๐ช๐ช๐๐๐๐
๏ฟฝฬ๏ฟฝ๐2 + ๐๐๐๐ ๐ช๐ช ๐๐ = ๐๐๐๐
Observe that ฮณi depends on the position only (and not on the velocity).
๐ผ๐ผ๐๐ ๐๐ ๏ฟฝฬ๏ฟฝ๐ + ๐ฝ๐ฝ๐๐ ๐๐ ๏ฟฝฬ๏ฟฝ๐2 + ๐พ๐พ๐๐ ๐๐ = ๐๐๐๐
Torques needed to execute the motion are thus:
๐๐๐๐ ๐ก๐ก = ๐ผ๐ผ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก + ๐ฝ๐ฝ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐2 ๐ก๐ก + ๐พ๐พ๐๐ ๐๐ ๐ก๐ก , ๐๐ = 1, โฆ ,๐๐, ๐ก๐ก โ 0,๐๐
Dynamic scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
In order to obtain a different parameterization of the trajectory, consider now a time scaling, for instance a linear one:
We obtain:
If ๐๐ > 1 the scaled trajectory is slower.
If ๐๐ < 1 the scaled trajectory is faster.
๐๐ ๐ก๐ก = ๏ฟฝ๐๐ ๐๐ , ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก = ๐๐ ๏ฟฝ๐๐โฒ ๐๐ , ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก = ๐๐2 ๏ฟฝ๐๐โณ ๐๐
๐๐ = ๐๐๐ก๐ก ๐๐ โ 0, ๐๐๐๐
where ()โฒ stands for derivative with respect to ๐๐.
we change the time scale and consider a new time ๐๐
t0
0
T
ฮธ kT
Dynamic scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
With the new parameterization, the torques become:
๐๐๐๐ ๐๐ = ๐ผ๐ผ๐๐ ๏ฟฝ๐๐ ๐๐ ๏ฟฝ๐๐โณ ๐๐ + ๐ฝ๐ฝ๐๐ ๏ฟฝ๐๐ ๐๐ ๏ฟฝ๐๐โฒ2 ๐๐ + ๐พ๐พ๐๐ ๏ฟฝ๐๐ ๐๐ =
= ๐ผ๐ผ๐๐ ๐๐ ๐ก๐ก๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก๐๐2
+ ๐ฝ๐ฝ๐๐ ๐๐ ๐ก๐ก๏ฟฝฬ๏ฟฝ๐2 ๐ก๐ก๐๐2
+ ๐พ๐พ๐๐ ๐๐ ๐ก๐ก =
=1๐๐2
๐ผ๐ผ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก + ๐ฝ๐ฝ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐2 ๐ก๐ก + ๐พ๐พ๐๐ ๐๐ ๐ก๐ก =
=1๐๐2
๐๐๐๐ ๐ก๐ก โ ๐๐๐๐ ๐๐ ๐ก๐ก + ๐๐๐๐ ๐๐ ๐ก๐ก
Then: ๐๐๐๐ ๐๐ โ ๐๐๐๐ ๐๐ =1๐๐2
๐๐๐๐ ๐ก๐ก โ ๐๐๐๐ ๐ก๐ก
๐๐๐๐ ๐ก๐ก = ๐ผ๐ผ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐ ๐ก๐ก + ๐ฝ๐ฝ๐๐ ๐๐ ๐ก๐ก ๏ฟฝฬ๏ฟฝ๐2 ๐ก๐ก + ๐พ๐พ๐๐ ๐๐ ๐ก๐ก , ๐๐ = 1, โฆ ,๐๐, ๐ก๐ก โ 0,๐๐
Dynamic scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
With a new parameterization of the trajectory it is not necessary to compute again the dynamics of the system;
New torques are obtained, apart from the gravitational term (which does not depend on the parameterization), multiplying the torques obtained with the original trajectory by the factor โ1 ๐๐2;
The travel time of the new trajectory is ๐๐๐๐.
t0
0
T
ฮธ kT0 ฯ ฯmax
q
q=q(ฯ)
๐๐๐๐ ๐๐ โ ๐๐๐๐ ๐๐ =1๐๐2
๐๐๐๐ ๐ก๐ก โ ๐๐๐๐ ๐ก๐ก
Dynamic scaling
Control of industrial robots โ Motion planning โ Paolo Rocco
Consider a two d.o.f. manipulator subjected to a trajectory which generates the following torques:
tU1
โU1
tU2
โU2
ฯ1 ฯ2 To scale the trajectory we compute the value:
๐๐2 = max 1,๐๐1๐๐1
,๐๐2๐๐2
โฅ 1
New torques will be realizable (๐๐ ๐๐ = โ๐๐ ๐ก๐ก ๐๐2) and at least one of them will saturate in one point.Scaling the trajectory in order to avoid that the torque exceeds the maximum value in a given interval may excessively slow down the execution: we can resort in this case to a variable scaling (i.e. applied only in those intervals where there is torque saturation).
Dynamic scaling: example
Control of industrial robots โ Motion planning โ Paolo Rocco
So far we have considered conditions only on the initial and final points of the trajectory.We will now consider the more general situation where intermediate points have to be interpolated at given instants:
๐ก๐ก1๐ก๐ก2โฎ
๐ก๐ก๐๐โ1๐ก๐ก๐๐
โ
๐๐1๐๐2โฎ
๐๐๐๐โ1๐๐๐๐
Interpolation of points
Control of industrial robots โ Motion planning โ Paolo Rocco
The problem of finding a trajectory that passes through n points can be solved adopting a polynomial function of degree ๐๐ โ 1:
๐๐ =
๐๐1๐๐2โฎ
๐๐๐๐โ1๐๐๐๐
=
1 ๐ก๐ก1 โฏ ๐ก๐ก1๐๐โ1
1 ๐ก๐ก2 โฏ ๐ก๐ก2๐๐โ1
โฎ1 ๐ก๐ก๐๐โ1 โฏ ๐ก๐ก๐๐โ1๐๐โ1
1 ๐ก๐ก๐๐ โฏ ๐ก๐ก๐๐๐๐โ1
๐๐0๐๐1โฎ
๐๐๐๐โ2๐๐๐๐โ1
= ๐๐๐๐
Given the values ๐ก๐ก๐๐, ๐๐๐๐, ๐๐ = 1,โฏ ,๐๐ vectors ๐๐, ๐๐ and matrix ๐๐ (Vandermonde matrix) are built as:
It follows: ๐๐ = ๐๐โ1๐๐ (matrix ๐๐ is always invertible if ๐ก๐ก๐๐ > ๐ก๐ก๐๐โ1, ๐๐ = 1,โฏ ,๐๐)
Interpolation with a polynomial
๐๐ ๐ก๐ก = ๐๐0 + ๐๐1๐ก๐ก + ๐๐2๐ก๐ก2 + โฏ+ ๐๐๐๐โ1๐ก๐ก๐๐โ1
Control of industrial robots โ Motion planning โ Paolo Rocco
-2 0 2 4 6 8 10 12-10
0
10
20
30
40
50
deg
Position
-2 0 2 4 6 8 10 12-20
-10
0
10
20
30
t(s)
deg/
s
Velocity
-2 0 2 4 6 8 10 12-40
-30
-20
-10
0
10
t(s)
deg/
s2
Acceleration
๐ก๐ก1 = 0 ๐ก๐ก2 = 2 ๐ก๐ก3 = 4 ๐ก๐ก4 = 8 ๐ก๐ก5 = 10๐๐1 = 10ยฐ ๐๐2 = 20ยฐ ๐๐3 = 0ยฐ ๐๐4 = 30ยฐ ๐๐5 = 40ยฐ
Interpolation with a polynomial
Control of industrial robots โ Motion planning โ Paolo Rocco
A clear advantage of the polynomial interpolation is that function ๐๐ ๐ก๐ก has continuous derivatives of all the orders inside the interval ๐ก๐ก1 ๐ก๐ก๐๐ .
However the method is not efficient from a numerical point of view: as the number n of points increases, the condition number ๐๐ (ratio between the maximum and minimum eigenvalue) of the Vandermondematrix ๐๐ increases too, making the inversion problem numerically ill-conditioned.
If, for example, ๐ก๐ก๐๐ = ๐๐๐๐
, ๐๐ = 1,โฏ ,๐๐) :
Other, more efficient, methods exist to compute the coefficient of the polynomial, however the numerical difficulties stand for high values of ๐๐.
16117 10139.110032.410519.137.492443.68687.981.15
2015106543
โ โ โ k
n
Interpolation with a polynomial
Control of industrial robots โ Motion planning โ Paolo Rocco
Regardless the numerical difficulties, the interpolation of n points with only one polynomial of degree ๐๐ โ 1 has other disadvantages:
1. the degree of the polynomial depends on ๐๐ and, for large values of ๐๐, the amount of computations might be remarkable;
2. changing a single point ๐ก๐ก๐๐ ,๐๐๐๐ implies to compute again the entire polynomial;3. adding a final point ๐ก๐ก๐๐+1,๐๐๐๐+1 implies the use of a higher degree polynomial and thus the
computation of all the coefficients; 4. the obtained solution in general presents undesired oscillations
An alternative, instead of using a single polynomial of degree ๐๐ โ 1, is to use ๐๐ โ 1 polynomials of degree ๐๐ (typically lower), each of which defined in an interval of the trajectory.The degree ๐๐ of the polynomials is usually equal to 3 (pieces of cubic trajectories).A first, and obvious, way to proceed is to assign positions and velocities in all the points and then to compute the coefficients of the cubic polynomials between two consecutive points.
Interpolation with a polynomial
Control of industrial robots โ Motion planning โ Paolo Rocco
-2 0 2 4 6 8 10 12-10
0
10
20
30
40
50
deg
Position
-2 0 2 4 6 8 10 12-20
-10
0
10
20
t(s)
deg/
s
Velocity
-2 0 2 4 6 8 10 12-40
-20
0
20
40
t(s)
deg/
s2
Acceleration
๐ก๐ก1 = 0 ๐ก๐ก2 = 2 ๐ก๐ก3 = 4 ๐ก๐ก4 = 8 ๐ก๐ก5 = 10๐๐1 = 10ยฐ ๐๐2 = 20ยฐ ๐๐3 = 0ยฐ ๐๐4 = 30ยฐ ๐๐5 = 40ยฐ๏ฟฝฬ๏ฟฝ๐1 = 0ยฐ/๐ ๐ ๏ฟฝฬ๏ฟฝ๐2 = โ10ยฐ/๐ ๐ ๏ฟฝฬ๏ฟฝ๐3 = 10ยฐ/๐ ๐ ๏ฟฝฬ๏ฟฝ๐4 = 3ยฐ/๐ ๐ ๏ฟฝฬ๏ฟฝ๐5 = 0ยฐ/๐ ๐
Interpolation with cubics
Control of industrial robots โ Motion planning โ Paolo Rocco
If only the intermediate positions are specified, and not the intermediate velocities, these can be assigned with rules like:
๏ฟฝฬ๏ฟฝ๐1 = 0
๏ฟฝฬ๏ฟฝ๐๐๐ = ๏ฟฝ0 sign ๐ ๐ ๐๐ โ sign ๐ ๐ ๐๐+1
๐ ๐ ๐๐ + ๐ ๐ ๐๐+12
sign ๐ ๐ ๐๐ = sign ๐ ๐ ๐๐+1๏ฟฝฬ๏ฟฝ๐๐๐ = 0
where:
๐ ๐ ๐๐ =๐๐๐๐ โ ๐๐๐๐โ1๐ก๐ก๐๐ โ ๐ก๐ก๐๐โ1
is the slope in the interval ๐ก๐ก๐๐โ1, ๐ก๐ก๐๐
Interpolation with cubics
Control of industrial robots โ Motion planning โ Paolo Rocco
-2 0 2 4 6 8 10 12-10
0
10
20
30
40
50
deg
Position
-2 0 2 4 6 8 10 12-20
-10
0
10
20
t(s)
deg/
s
Velocity
-2 0 2 4 6 8 10 12-30
-20
-10
0
10
20
30
t(s)
deg/
s2
Acceleration
๐ก๐ก1 = 0 ๐ก๐ก2 = 2 ๐ก๐ก3 = 4 ๐ก๐ก4 = 8 ๐ก๐ก5 = 10๐๐1 = 10ยฐ ๐๐2 = 20ยฐ ๐๐3 = 0ยฐ ๐๐4 = 30ยฐ ๐๐5 = 40ยฐ
Interpolation with cubics
Control of industrial robots โ Motion planning โ Paolo Rocco
The interpolation with cubic polynomials generates a trajectory which presents a discontinuity in the acceleration in the intermediate points.
In order to avoid this problem, while keeping cubic interpolation, we must avoid to assign specific values of velocity in the intermediate points, assigning just the continuity of velocities and accelerations (and of course positions) in these points.
The trajectory which is obtained this way is called spline (smooth path line).
The spline is the minimum curvature interpolating function, given some conditions on continuity of derivatives.
Spline
Control of industrial robots โ Motion planning โ Paolo Rocco
4 ๐๐ โ 1 โ 2 ๐๐ โ 1 โ 2 ๐๐ โ 2 = 2We thus have:
A (not unique) way to use these 2 degrees of freedom consists in assigning suitable initial and final conditions on the velocity.
2 ๐๐ โ 1 conditions of passage through the points (each cubic has to interpolate the points at its boundaries)
๐๐ โ 2 conditions on continuity of velocities in the intermediate points ๐๐ โ 2 conditions on continuity of accelerations in the intermediate points
residual degrees of freedom.
๐๐ ๐ก๐ก = ๐๐0 + ๐๐1๐ก๐ก + ๐๐2๐ก๐ก2 + ๐๐3๐ก๐ก3Since with ๐๐ points we need ๐๐ โ 1 polynomials like:
each of which has 4 coefficients, the total number of coefficients to be computed is 4 ๐๐ โ 1 . The conditions to be imposed are:
Spline: conditions
Control of industrial robots โ Motion planning โ Paolo Rocco
We want to determine a function:
๐๐ ๐ก๐ก = ๐๐๐๐ ๐ก๐ก , ๐ก๐ก โ ๐ก๐ก๐๐ , ๐ก๐ก๐๐+1 , ๐๐ = 1, โฆ ,๐๐ โ 1๐๐๐๐ ๐๐ = ๐๐๐๐0 + ๐๐๐๐1๐๐ + ๐๐๐๐2๐๐2 + ๐๐๐๐3๐๐3, ๐๐ โ 0,๐๐๐๐ ๐๐ = ๐ก๐ก โ ๐ก๐ก๐๐ , ๐๐๐๐ = ๐ก๐ก๐๐+1 โ ๐ก๐ก๐๐
with the conditions:
where the quantities ๐ฃ๐ฃ๐๐ , ๐๐ = 2, โฆ ,๐๐ โ 1 are not specified.The problem consists in finding the coefficients ๐๐๐๐๐๐.
Spline: analytical position of the problem
๐๐๐๐ 0 = ๐๐๐๐ , ๐๐๐๐ ๐๐๐๐ = ๐๐๐๐+1๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐+1 0 = ๐ฃ๐ฃ๐๐+1๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐๐๐ = ๏ฟฝฬ๏ฟฝ๐๐๐+1 0๏ฟฝฬ๏ฟฝ๐1 0 = ๐ฃ๐ฃ1, ๏ฟฝฬ๏ฟฝ๐๐๐โ1 ๐๐๐๐โ1 = ๐ฃ๐ฃ๐๐
๐๐ = 1, โฆ ,๐๐ โ 1๐๐ = 1, โฆ ,๐๐ โ 2๐๐ = 1, โฆ ,๐๐ โ 2
Control of industrial robots โ Motion planning โ Paolo Rocco
Assume initially that the velocities ๐ฃ๐ฃ๐๐ ,๐๐ = 2, โฆ ,๐๐ โ 1 in the intermediate points are known. In this way, for each cubic polynomial we have four boundary conditions on position and velocity, which give rise to the system:
Solving the system yields:
Spline: algorithm
๐๐๐๐ 0 = ๐๐๐๐0 = ๐๐๐๐๏ฟฝฬ๏ฟฝ๐๐๐ 0 = ๐๐๐๐1 = ๐ฃ๐ฃ๐๐๐๐๐๐ ๐๐๐๐ = ๐๐๐๐0 + ๐๐๐๐1๐๐๐๐ +๐๐๐๐2 ๐๐๐๐2 +๐๐๐๐3 ๐๐๐๐3 = ๐๐๐๐+1๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐๐๐ = ๐๐๐๐1 +2๐๐๐๐2 ๐๐๐๐ +3๐๐๐๐3 ๐๐๐๐2 = ๐ฃ๐ฃ๐๐+1
๐๐๐๐0 = ๐๐๐๐๐๐๐๐1 = ๐ฃ๐ฃ๐๐
๐๐๐๐2 =1๐๐๐๐
3 ๐๐๐๐+1 โ ๐๐๐๐๐๐๐๐
โ 2๐ฃ๐ฃ๐๐ โ ๐ฃ๐ฃ๐๐+1
๐๐๐๐3 =1๐๐๐๐2
2 ๐๐๐๐ โ ๐๐๐๐+1๐๐๐๐
+ ๐ฃ๐ฃ๐๐ + ๐ฃ๐ฃ๐๐+1
Control of industrial robots โ Motion planning โ Paolo Rocco
Obviously the velocities ๐ฃ๐ฃ๐๐ , ๐๐ = 2, โฆ ,๐๐ โ 1 must be computed. Let us impose the continuity of the accelerations in the intermediate points:
๏ฟฝฬ๏ฟฝ๐๐๐ ๐๐๐๐ = 2๐๐๐๐2 + 6๐๐๐๐3๐๐๐๐ = 2๐๐๐๐+1,2 = ๏ฟฝฬ๏ฟฝ๐๐๐+1 0 , ๐๐ = 1, โฆ ,๐๐ โ 2
By substituting the expressions for the coefficients ๐๐๐๐2, ๐๐๐๐3, ๐๐๐๐+1,2 and multiplying by โ๐๐๐๐๐๐๐๐+1 2 we obtain:
๐๐๐๐+1๐ฃ๐ฃ๐๐ + 2 ๐๐๐๐+1 + ๐๐๐๐ ๐ฃ๐ฃ๐๐+1 + ๐๐๐๐๐ฃ๐ฃ๐๐+2 =3
๐๐๐๐๐๐๐๐+1๐๐๐๐2 ๐๐๐๐+2 โ ๐๐๐๐+1 + ๐๐๐๐+12 ๐๐๐๐+1 โ ๐๐๐๐
Spline: algorithm
Control of industrial robots โ Motion planning โ Paolo Rocco
In matrix form:
๐๐๐๐+1๐ฃ๐ฃ๐๐ + 2 ๐๐๐๐+1 + ๐๐๐๐ ๐ฃ๐ฃ๐๐+1 + ๐๐๐๐๐ฃ๐ฃ๐๐+2 =3
๐๐๐๐๐๐๐๐+1๐๐๐๐2 ๐๐๐๐+2 โ ๐๐๐๐+1 + ๐๐๐๐+12 ๐๐๐๐+1 โ ๐๐๐๐
๐๐2 2 ๐๐1 + ๐๐2 ๐๐10 ๐๐3 2 ๐๐2 + ๐๐3 ๐๐2
โฎ๐๐๐๐โ2 2 ๐๐๐๐โ3 + ๐๐๐๐โ2 ๐๐๐๐โ3 0
๐๐๐๐โ1 2 ๐๐๐๐โ2 + ๐๐๐๐โ1 ๐๐๐๐โ2
๐ฃ๐ฃ1๐ฃ๐ฃ2โฎ
๐ฃ๐ฃ๐๐โ1๐ฃ๐ฃ๐๐
=
๐๐1๐๐2โฎ
๐๐๐๐โ3๐๐๐๐โ2
where constants ๐๐๐๐ depend only on the intermediate positions and on the lengths of the intervals, all known quantities. Since the velocities ๐ฃ๐ฃ1 and ๐ฃ๐ฃ๐๐ are known (they are specified as initial data), by eliminating the related columns we have:
Spline: algorithm
Control of industrial robots โ Motion planning โ Paolo Rocco
2 ๐๐1 + ๐๐2 ๐๐1๐๐3 2 ๐๐2 + ๐๐3 ๐๐2
โฎ๐๐๐๐โ2 2 ๐๐๐๐โ3 + ๐๐๐๐โ2 ๐๐๐๐โ3
๐๐๐๐โ1 2 ๐๐๐๐โ2 + ๐๐๐๐โ1
๐ฃ๐ฃ2โฎ
๐ฃ๐ฃ๐๐โ1
=3
๐๐1๐๐2๐๐12 ๐๐3 โ ๐๐2 + ๐๐22 ๐๐2 โ ๐๐1 โ ๐๐2๐ฃ๐ฃ13
๐๐2๐๐3๐๐22 ๐๐4 โ ๐๐3 + ๐๐32 ๐๐3 โ ๐๐2
โฎ3
๐๐๐๐โ3๐๐๐๐โ2๐๐๐๐โ32 ๐๐๐๐โ1 โ ๐๐๐๐โ2 + ๐๐๐๐โ22 ๐๐๐๐โ2 โ ๐๐๐๐โ3
3๐๐๐๐โ2๐๐๐๐โ1
๐๐๐๐โ22 ๐๐๐๐ โ ๐๐๐๐โ1 + ๐๐๐๐โ12 ๐๐๐๐โ1 โ ๐๐๐๐โ2 โ ๐๐๐๐โ2๐ฃ๐ฃ๐๐ i.e an equation in the form ๐๐๐๐ = ๐๐
Spline: algorithm
Control of industrial robots โ Motion planning โ Paolo Rocco
Matrix ๐๐ is a dominant diagonal matrix and is always invertible provided that ๐๐๐๐ > 0;
Matrix ๐๐ has a tridiagonal structure: for these matrices there exist efficient numerical techniques (Gauss-Jordan method) for its inversion;
Once the inverse of ๐๐ is known we might compute velocities ๐ฃ๐ฃ2,โฏ , ๐ฃ๐ฃ๐๐โ1as:
which completely solves the problem.
๐๐ = ๐๐โ1๐๐
It is also possible to determine the spline with an alternative (yet completely equivalent) algorithm which computes the accelerations instead of the velocities in the intermediate points.
Spline: algorithm
Control of industrial robots โ Motion planning โ Paolo Rocco
-2 0 2 4 6 8 10 12-10
0
10
20
30
40
50
deg
Position
-2 0 2 4 6 8 10 12-20
-10
0
10
20
t(s)
deg/
s
Velocity
-2 0 2 4 6 8 10 12-20
-10
0
10
20
t(s)
deg/
s2
Acceleration
๐ก๐ก1 = 0 ๐ก๐ก2 = 2 ๐ก๐ก3 = 4 ๐ก๐ก4 = 8 ๐ก๐ก5 = 10๐๐1 = 10ยฐ ๐๐2 = 20ยฐ ๐๐3 = 0ยฐ ๐๐4 = 30ยฐ ๐๐5 = 40ยฐ
Spline: example
Control of industrial robots โ Motion planning โ Paolo Rocco
The overall travel time of a spline is given by:
๐๐ = ๏ฟฝ๐๐=1
๐๐โ1
๐๐๐๐ = ๐ก๐ก๐๐ โ ๐ก๐ก1
It is possible to conceive an optimization problem which minimizes the overall travel time. The problem is to determine the values ๐๐๐๐ so as to minimize ๐๐, with the constraints on the maximum velocities and accelerations. Formally:
min๐๐๐๐
๐๐ = ๏ฟฝ๐๐=1
๐๐โ1
๐๐๐๐
such that๏ฟฝฬ๏ฟฝ๐ ๐๐,๐๐๐๐ < ๐ฃ๐ฃmax ๐๐ โ 0,๐๐๏ฟฝฬ๏ฟฝ๐ ๐๐,๐๐๐๐ < ๐๐max ๐๐ โ 0,๐๐
It is then a non linear optimum problem with a linear cost function, which can be solved with operations research techniques.
Spline: travel time
Control of industrial robots โ Motion planning โ Paolo Rocco
An alternative, quite simple, way to handle the interpolation problem is to link the points with linear functions (segments). In order to avoid discontinuities in the velocity, the linear segments can be connected through parabolic bends
The resulting trajectory q(t) does not pass through any of the intermediate points, though it is close to them. In this case the intermediate points are called via points.
This picture is taken from the textbook:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009
Interpolation with linear segments
Control of industrial robots โ Motion planning โ Paolo Rocco
If we use a sequence of trajectories with trapezoidal velocity profile to interpolate points, we would obtain a motion which passes through these points with zero velocity (i.e. stopping).To avoid this we can start the planning of a trajectory before the end of the preceding one:
This picture is taken from the textbook:B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed., Springer, 2009
TVP and interpolation of points
Control of industrial robots โ Motion planning โ Paolo Rocco
Trajectory planning in the joint space yields unpredictable motions of the end-effector. When we want the motion to evolve along a predefined path in the operational space, it is necessary to plan the trajectory directly in this space.
Trajectory planning in the operational space entails both a path planning problem and a timing law planning problem: both the path and the timing law can be expressed analytically, as it will be shown in the following.
We will first address the trajectory planning for the position, and then we will concentrate on the orientation.
Trajectories in the operational space
Control of industrial robots โ Motion planning โ Paolo Rocco
Let us consider a parametric representation of a curve in space. The parameterization can be performed with respect to the natural coordinate (length of the arc of trajectory): ๐ฉ๐ฉ = ๐ฉ๐ฉ ๐ ๐
x
y
z
t
nb
ppi pf ๐ญ๐ญ =๐๐๐ฉ๐ฉ ๐ ๐ ๐๐๐ ๐
๐ง๐ง =โ๐๐2๐ฉ๐ฉ ๐ ๐ ๐๐๐ ๐ 2
โ๐๐2๐ฉ๐ฉ ๐ ๐ ๐๐๐ ๐ 2
๐๐ = ๐ญ๐ญ ร ๐ง๐ง
We can define the tangent, normal and binormal unit vectors:
unit tangent vector
unit normal vector (belongs to the osculating plane)
unit binormal vector
Path parameterization
Control of industrial robots โ Motion planning โ Paolo Rocco
As an example of path parameterization we can consider a segment in space (linear Cartesian path):
๐ฉ๐ฉ ๐ ๐ = ๐ฉ๐ฉ๐๐ +๐ ๐
๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐
๐๐๐ฉ๐ฉ๐๐๐ ๐ =
1๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐
๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐
๐๐2๐ฉ๐ฉ๐๐๐ ๐ 2 = 0
In this case it is not possible to define the frame (๐ญ๐ญ, ๐ง๐ง, ๐๐) uniquely.
pf
pi
Linear path
Control of industrial robots โ Motion planning โ Paolo Rocco
A linear path is completely characterized once two points in Cartesian space are given.
Linear path
Control of industrial robots โ Motion planning โ Paolo Rocco
Linear paths can be concatenated in order to obtain more elaborated paths.
The intermediate point between two consecutive segments can be considered as a via point, meaning that there is no need to pass and stop there.
During the over-fly, i.e. the passage near a via point, the path remains always in the plane specified by the two lines intersecting in the via point. This means that the problem of planning the over-fly is planar.
Formulas can be derived to define the blending (typically a parabolic one)
A
B
C
over-fly
Concatenation of linear paths
Control of industrial robots โ Motion planning โ Paolo Rocco
A parametric representation of a circumference of radius ๐๐ laying in a plane ๐ฅ๐ฅโฒ๐ฆ๐ฆโฒ and having the centre in the origin of such plane is:
๐ฉ๐ฉโฒ ๐ ๐ =๐๐ cos โ๐ ๐ ๐๐๐๐ sin โ๐ ๐ ๐๐
0
Defining:
๐๐ the vector that identifies the centre of the circumference in the base frame ๐๐ the rotation matrix from base frame to frame ๐ฅ๐ฅโฒ๐ฆ๐ฆโฒ๐ง๐งโฒ
the general parametric representation of a circumference in space is:
๐ฉ๐ฉ ๐ ๐ = ๐๐ + ๐๐๐ฉ๐ฉโฒ ๐ ๐
Circular path
Control of industrial robots โ Motion planning โ Paolo Rocco
The circular path can also be defined assigning three points in space belonging to the same plane:
Circular path
Control of industrial robots โ Motion planning โ Paolo Rocco
For the position trajectories, taking into account the parameterization of the path with respect to the natural coordinate ๐ฉ๐ฉ = ๐ฉ๐ฉ ๐ ๐ , we will assign the timing law through the function ๐ ๐ ๐ก๐ก .
In order to determine function ๐ ๐ ๐ก๐ก we can use any of the timing laws (polynomials, harmonic, trapezoidal velocity profile, spline, etc.) already studied. Also we notice that:
tt0
ss0
px=px(s)py=py(s)pz=pz(s)
๏ฟฝฬ๏ฟฝ๐ฉ = ๏ฟฝฬ๏ฟฝ๐ ๐๐๐ฉ๐ฉ๐๐๐ ๐
= ๏ฟฝฬ๏ฟฝ๐ ๐ญ๐ญ ๏ฟฝฬ๏ฟฝ๐ is then the norm of the velocity
For the segment: ๏ฟฝฬ๏ฟฝ๐ฉ =๏ฟฝฬ๏ฟฝ๐
๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐๐ฉ๐ฉ๐๐ โ ๐ฉ๐ฉ๐๐ = ๏ฟฝฬ๏ฟฝ๐ ๐ญ๐ญ
the time law ๐ ๐ ๐ก๐กtakes then an immediate meaning!
Position trajectories
Control of industrial robots โ Motion planning โ Paolo Rocco
For the orientation planning we might interpolate (e.g. linearly) the components of the unit vectors ๐ง๐ง ๐ก๐ก , ๐ฌ๐ฌ ๐ก๐ก and ๐๐ ๐ก๐ก .This procedure is however not advisable as the orthonormality of unit vectors cannot be guaranteed at all times.
An alternative way is to interpolate three Euler angles, using the following relations:
ฯ ๐ ๐ = ฯ๐๐ +๐ ๐
ฯ๐๐ โ ฯ๐๐ฯ๐๐ โ ฯ๐๐
ฯฬ =๏ฟฝฬ๏ฟฝ๐
ฯ๐๐ โ ฯ๐๐ฯ๐๐ โ ฯ๐๐
ฯฬ =๏ฟฝฬ๏ฟฝ๐
ฯ๐๐ โ ฯ๐๐ฯ๐๐ โ ฯ๐๐
We can use any timing law s(t) on the natural parameter.The angular velocity ฯ, which is linearly related to ๏ฟฝฬ๏ฟฝ๐, has a continuous variation of the amplitude.
Poor predictability and understanding of the intermediate orientation!
Orientation trajectories
Control of industrial robots โ Motion planning โ Paolo Rocco
Orientation can also be planned by resorting to the axis/angle representation: if we assign two frames with the same origin but different orientation, it is always possible to determine a unit vector ๐ซ๐ซ such that the second frame is obtained from the first one through a rotation of an angle ๐๐๐๐ around the axis of such unit vector. Let ๐๐๐๐ and ๐๐๐๐ the rotation matrices, with respect to the base frame, of the initial frame and of the final frame. The rotation matrix between the two frames, with related axis/angle representation is thus:
๐๐๐๐๐๐ = ๐๐๐๐๐๐๐๐๐๐ =๐๐11 ๐๐12 ๐๐13๐๐21 ๐๐22 ๐๐23๐๐31 ๐๐32 ๐๐33
๐๐๐๐ = cosโ1๐๐11 + ๐๐22 + ๐๐33 โ 1
2
๐ซ๐ซ =1
2 sin ๐๐๐๐
๐๐32 โ ๐๐23๐๐13 โ ๐๐31๐๐21 โ ๐๐12
trace of matrix ๐๐
Orientation trajectories
Control of industrial robots โ Motion planning โ Paolo Rocco
Define with ๐๐๐๐ ๐ก๐ก the matrix that describes the transition from ๐๐๐๐ to ๐๐๐๐.Then:
๐๐๐๐ 0 = ๐๐, ๐๐๐๐ ๐ก๐ก๐๐ = ๐๐๐๐๐๐
Matrix ๐๐๐๐ ๐ก๐ก can be interpreted as ๐๐๐๐ ๐๐ ๐ก๐ก , ๐ซ๐ซ , where:
๐ซ๐ซ is constant and can be computed from the elements of ๐๐๐๐๐๐
๐๐ ๐ก๐ก can be made variable with time, through a timing law, with ๐๐ 0 = 0, ๐๐ ๐ก๐ก๐๐ = ๐๐๐๐In order to characterize the orientation in the base frame it is then enough to compute: ๐๐ ๐ก๐ก = ๐๐๐๐๐๐๐๐ ๐ก๐ก
Orientation trajectories
๐๐๐๐
๐๐๐๐
๐๐๐๐
Control of industrial robots โ Motion planning โ Paolo Rocco
Alternatively we can use unit quaternions to specify the orientation.They are defined as:
with:
where ๐๐ and ๐ซ๐ซ have the same meaning as with the axis/angle representation, but the representation singularity has been removed:
๐๐2 + ๐๐๐๐2 + ๐๐๐ฆ๐ฆ2 + ๐๐๐ง๐ง2 = 1
Orientation trajectories
โ = ๐๐, ๐บ๐บ
๐๐ = cos๐๐2
๐บ๐บ = sin๐๐2๐ซ๐ซ ๐๐ =
12
๐๐11 + ๐๐22 + ๐๐33 + 1
๐บ๐บ =12
sgn ๐๐32 โ ๐๐23 ๐๐11 โ ๐๐22 โ ๐๐33 + 1sgn ๐๐13 โ ๐๐31 ๐๐22 โ ๐๐33 โ ๐๐11 + 1sgn ๐๐21 โ ๐๐12 ๐๐33 โ ๐๐11 โ ๐๐22 + 1
Control of industrial robots โ Motion planning โ Paolo Rocco
InverseKinematicstarget
stateINSTRUCTION
STACK
TrajGen
Axiscontroller
state update
Instruction stack: list of instructions to be executed, specified using the proprietary programming language
Trajectory generation: converts an instruction into a trajectory to be executed Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed) Axis controllers & drives: closes the control loop ensuring tracking performance
Elements of a motion planning and control system
Control of industrial robots โ Motion planning โ Paolo Rocco
Inverse kinematics has already been discussed with reference to the study of the kinematics of the (possibly redundant) manipulator.
J#(q)K ++โ
rd +
rd.
InโJ#(q)J(q)
q0.
++ q. q
f(q)r
Options are:
Closed form analytic solution (whenever possible)
Numerical solution through (pseudo) inverse of the Jacobian
Inverse kinematics
Control of industrial robots โ Motion planning โ Paolo Rocco
InverseKinematicstarget
stateINSTRUCTION
STACK
TrajGen
Axiscontroller
state update
Instruction stack: list of instructions to be executed, specified using the proprietary programming language
Trajectory generation: converts an instruction into a trajectory to be executed Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed) Axis controllers & drives: closes the control loop ensuring tracking performance (see next set of
slides)
Elements of a motion planning and control system