1. Chapter 4 . Trajectory planning and Inverse kinematics
Trajectory Planning & Inverse Kinematics

2. Table of Contents Introduction Trajectory planning Inverse kinematics
Velocity kinematics

3. Introduction For a given task to the robot,
 How to change the end-effector frame?
Trajectory planning: Method of moving the frames
Inverse kinematics: For the desired location of the frame, obtain the joint variables.

4. Introduction Inverse Kinematics [Fig. 4-1] Work space of the robot arm
Obstacle
Via point
[Fig. 4-1] Work space of the robot arm
Inverse Kinematics

5. 4.1 Trajectory planning
Through Point : The end-effector should pass through this point.
Via Point : The end-effector may move around this point.
For more efficient task executions,
=> Path + Trajectory planning

6. Difference between Manipulator and Mobile Robot
M  End-effector
M. R  Whole body is moving
M  Configuration changes
M.R  Location changes with solid body
M  6-DOF
M.R  3-DOF on the plane
M  In the fixed environment
M.R  In the dynamically changing environment

7. Cont. M  Object recognition M.R  Environment recognition
M  The size is very critical
M  Trajectory planning is deterministic
M.R  Location is a big issue because of dead reckoning errors

8. Trajectory planning of manipulator
Computation of trajectory for a given task
 To move EE to the desired
position/orientation from the current location
 Represent as homogeneous transformation matrix
Trajectory: Set of position/orientation of EE
as a function of time
 Position, velocity, and acceleration trajectories
(Ex: painting, welding)

9. Trajectory planning in Cartesian space
Trajectory in Cartesian or joint space should be determined
Considerations:

10. Method 1. Change of Cartesian to joint coordinates in every sampling
 Comparison of the desired and actual paths
Forward kinematics
 Error in Joint space
Inverse kinematics
Shortage: Computational complexity limits sampling cycle.

11. Method 2. Joint space trajectory
 Considering the Max. velocity of each joint
 Using forward kinematics, the Cartesian space error
can be checked.

12. Method 3. Using polynomial equations for the joint space trajectory
 Manipulator may generate the smooth motion.

13. Position control + Real time obstacle avoidance
Path control mode
Obstacle
Yes No
Path
Constraints
off-line
collision-free
path planning
plus on-line
path tracking
Position control + Real time obstacle avoidance
Position control

14. Trajectory planning Moving EE from current to the desired position:
Path Planning
Given path
Trajectory Planning
Dynamic constrains of the manipulator
[Fig. 4-2] Trajectory planning

15. Joint space 1. 3rd order polynomial
 Within a given time, arriving to the desired position from the current position.
Four constraints: position & velocity of current & desired
To keep the 3rd order polynomial,
(4.1)
(4.2)

16. Via points 2. 3rd order polynomial with via points
 Non stop at the via points. Therefore using the constraints,
(4.3)
Specification of velocities
① User
② Heuristic rules
③ Continuous acceleration at the via points

17. Via points 3. High order polynomial
 Position, velocity, and acceleration at the start and end points:
4. Polynomial & linear function
 Polynomial for the start and end with constant accelerations to make smooth change of the velocity
5. Polynomial & linear function with via point
 Virtual via points can be used
(4.4)

18. Cartesian coordinates method
Transformation from the Cartesian coordinates to joint
coordinates may cause singularity problem.
Considering some examples, singularity can be understood.

19. Ex. 1. (Out of workspace) ① EE cannot reach some portion of the path
[Fig. 4-3] Example 1

20. Ex. 2. Singular configuration
② Very high joint rates near at the singular
[Fig. 4-4] Example 2

21. Ex. 3. Multiple solutions
③ Multiple solutions
[Fig. 4-5] Example 3

22. 4.2 Path generation 1. In the middle of tasks,
- By using the path generated during the task execution, the trajectory, , is generated and used for the control system.
- The path in the Cartesian space is transformed to the corresponding joint values.
Position  Inverse kinematics, Velocity  Inverse Jacobian,
Acceleration  Inverse Jacobian and the derivative

23. Path generation 2. Using the dynamic model
- Minimum time trajectory can be obtained using dynamics and velocity-torque curve of the actuators.
3. Using the programming language
- AL: Robot programming language developed at Stanford.
ex) Move ARM to C with duration = 3 * seconds;
Move ARM to C;
Move ARM to C via B;

24. [Fig. 4-6] Moving trajectory of the robot arm
4.3 Trajectory planning
Moving a block on the table to the other position
[Fig. 4-6] Moving trajectory of the robot arm

25. Trajectory planning of robot
Task is matching
1 step: to with grasping.
2 step: to
Motion of robot
- Timely change of HT matrix
- Timely change of path

26. [Fig. 4-7] Change in Cartesian space an joint space
Ex 4.
Specifying joint position, velocity and acceleration in time domain
For an example,
[Fig. 4-7] Change in Cartesian space an joint space

27. Constraints While moving from A to B, a certain point should be passed
through (through point)
When and are specified,
, are also specified
However, are limited by the torques

28. Minimum time trajectory planning
[Fig. 4-8] Acceleration trajectory, velocity and position

29. [Fig. 4-9] Path planning to avoid the obstacles
Mobile robot
The robot is considered as a Point Object. The obstacles
01,02,03 should be avoided
(4.5)
[Fig. 4-9] Path planning to avoid the obstacles

30. [Fig. 4-10] Path planning to avoid the obstacles
Real mobile robot
Expand the obstacles considering the size of the mobile robot
 Obstacle avoidance & minimum time trajectory planning
[Fig. 4-10] Path planning to avoid the obstacles

31. A* algorithm

32. O:Open List, C: Closed List

36. Optimal path

37. 4.4 Inverse kinematics
The position/orientation of end-effector is given w.r.t. the reference frame.
 Obtain the joint angles for the desired location
Sequence of inverse kinematics
1. Transform the EE coordinates to Base coordinates
2. Solve the mathematics
EE position in Cartesian space  joint space values

38. Cautions If there are more than 6 joints, multiple solutions exist.
If less than 6 joints are there, then there might be no solution of where n < 6 corresponding to .
Among the nine variables in the rotation matrix,
only three of them are independent.
(4.6)

39. Solving the inverse kinematics
For the given homogeneous transformation matrix, obtaining
is the inverse kinematics.
For the Stanford manipulator,
(4.7)

40. Inverse kinematics
(4.8)
From the above 12 equations, the angles can be obtained.

41. Closed form solution
Closed form solution is obtained as an equation as follow:
Advantages of closed form solution:
1. The solution can be obtained quickly
2. Existence of multiple solutions can be identified
(4.9)

42. Resolving multiple solution
Check the solution whether those are feasible or not
--- Constraints also can be checked
The optimal solution can be selected and used

43. Inverse kinematics (special case)
Using the geometrical features of the robot, it can be solved easily
The special case is introduced
Let’s consider PUMA robot

44. Separation of upper and lower bodies.
Idea
[Fig. 4-11] Structure of PUMA Robot

45. Vector analysis d vector: From the origin of frame {4} to frame {6}
Then,
Length of d is constant and the direction is
Therefore,
(4.10)

46. Obtaining (4.11) is given as is ftn of
Even though 4th joint rotates, does not changes
(4.11)
(4.12)

47. Obtaining By the three equations, are obtained
Three unknowns can be obtained by this equation
(4.13)

48. Velocity inverse kinematics
Cartesian velocity, and joint space velocity,
are related by a Jacobian matrix as
When m=n, J -1can be multiplied at both sides to obtain the joint space
velocity as
Three cases can be considered:
(4.14)
(4.15)

49. Three cases
Case 1 ) and ,
singular case, arbitrary motion is not possible
Case 2 ) , minimum error solution
The solution does not exist generally
Case 3 ) , redundancy resolution
Redundant Manipulator
Among the multiple solutions,
an optimal solution needs to be selected .

50. Minimum error solution
(4.16)

51. Redundancy resolution
(4.17)
Z could be an arbitrary vector in the joint space

52. [Fig. 4-12] Four solution of PUMA 560
FOUR SOLUTIONS OF PUMA 560
[Fig. 4-12] Four solution of PUMA 560

53. [Fig. 4-13] Inverse kinematics of 3 link planar manipulator
Example 4.4
Inverse kinematics of 3 link planar manipulator
[Fig. 4-13] Inverse kinematics of 3 link planar manipulator

54. [Table 4-1] Link parameter of 3 link planar manipulator
Link Parameter Table
Link 1 2 3
[Table 4-1] Link parameter of 3 link planar manipulator

55. Homogeneous T.M
It is simplified as
(4.18)
(4.19)

56. Obtaining
where
(4.20)
Using ,
(4.21)
. So can be obtained

57. Obtaining are represented as follows: (4.22) Where, and are known
Therefore can be obtained
(4.22)

58. Obtaining
, using the and with the
(given), can be obtained

59. Homework