A word from me:
I was really excited about creating a complete guide for Trajectory Planning for DPR, and I must say, it was quite a challenging task. However, I became pretty obsessive about finding a solution and felt driven to share it with others as an open-source ... thing. I hope you'll find it helpful.
If you need to ask any questions, here's my email: [email protected]
And here are some of my Articles regarding these research: ...WIP...
Overview:
- Introduction
- Trajectory Planning
- point-to-point trajectory planning
- 3-4-5 interpolating polynomial
- 4-5-6-7 interpolating polynomial
- Trapezoidal method
- multi-point trajectory planning
- Higher Order Polynomials
- Cubic-Spline
- Improved Cubic-Spline
- Visual Results
- point-to-point trajectory planning
- Experimental
- References
History
Delta Parallel Robots (DPR) are part of the third generation of industrial robots, have evolved since the 1950s. Their parallel kinematic structure and high-speed capabilities make them ideal for precise tasks, particularly in pick-and-place operations. This article (or something like an article) sheds light on trajectory planning for DPRs, focusing on smooth motion for the End-Effector while minimizing deviations [1].
Algorithms
The trajectory-planning problem of a DPR can be tackled [3][4][5][6][7] using various algorithms.
- 5th and 7th order polynomials
- Cubic Splines
- Higher Order polynomials
- 4th, 6th, and 7th order B-Spline
- Lame’s Curve
- Pythagorean-Hodograph Curves
- Particle Swarm Optimization (PSO)
- butterfly optimization algorithm
- Trapezoidal Algorithm
Importance
Efficient trajectory planning is vital for DPRs successfuly managing their tasks. Smooth paths for the End-Effector (EE) while respecting jerk constraints ensure precise movement, avoiding mechanical stress. Our ambition is to find a good path generated for different applications.
FK and IK
The code to Inverse and Forward Kinematics can be found in the Delta Robot file. And Here's the source of the code.
Trajectory planning generates a time-based sequence of values, respecting the imposed constraints, to specify the position and orientation of the EE at any given time [8].
Point-to-Point Trajectory Planning refers to the process of generating smooth and coordinated paths that involves moving from a starting point to a single target location.
Math:
When interpolating between given initial and final values of the joint variable
Here,
In this context, it is important to note that
To establish desired constraints on the generated path, initial and final positions, velocities, and accelerations can be set. By applying the following conditions:
a system of six equations with six unknowns can be solved. The resulting values are:
Thus, the polynomial takes the form:
This representation allows for smooth and controlled joint variable interpolation, satisfying the prescribed constraints [9].
Discussion:
One significant drawback is the lack of explicit constraints on jerk, which refers to the rate of change of acceleration. The absence of jerk constraints can result in undesirable mechanical stress and instability, particularly at the start and end points of the trajectory where jerk values may be unbounded. The code can be found in the path planning file in the function point_to_point_345
Math:
If we consider
In this formula,
The constraints for the path generated using this method include setting the initial and final position, velocity, acceleration, and jerk. By incorporating the following conditions:
By solving this system of eight equations with eight unknowns, we can determine the values of the coefficients:
As a result, the polynomial will take the form [9]:
The result of this method is shown in the figure below. As explained, the advantage of this method compared to 3-4-5 interpolating polynomial is that the jerk is bounded at the initial and final points.
Discussion:
The 4-5-6-7 interpolating polynomial offers an improvement over the 3-4-5 interpolating polynomial by incorporating higher-order terms.
The code can be found in the path planning file in the function point_to_point_4567
Math::
Like the previous methods, the goal here is to basically use a trapezoidal diagram as a way to interpolate the velocity profile between the values of
For the sake of simplicity we say that
Since
Implementing this sequence with a Python script, we can get the results show as below [10]:
Discussion::
This method has the same problem of 3-4-5 method, but instead of start and finishing point, the problem is at trapezoidal_ptp
Multi-Point Trajectory Planning involves generating a path that include multiple target locations.
Math
Remember how in the 4-5-6-7 interpolating polynomial we used a 7th order polynomial to constraint the jerk, acceleration, velocity and position of two points? In theory we can do that with any number of points. say we have
- initial and final velocity equals zero
- initial and final acceleration equals zero
- initial and final jerk equals zero
this means we'll be having
let's say we have the polynomial as:
and the conditions are for the polynomial
These conditions will give us 9 equations as well as 9 coefficients to calculate which make up a system of linear equations. the equation solution is uploaded in this file and the final answers are:
Discussion
Using this method isn't all that appreciated anyways because for the larger numder of points we'll have some problems. Here are some of those problems listed:
- Less Sensitivity to Data Perturbations: High-degree polynomials are highly sensitive to changes in data points. Even small adjustments in the input data can significantly affect the resulting polynomial.
- Avoiding Overfitting: High-degree polynomials can lead to overfitting the data, capturing noise rather than the underlying trend.
- Numerical Efficiency: Solving systems of equations involving high-degree polynomials can be computationally expensive and may lead to numerical issues. In contrast, solving cubic splines is relatively efficient and numerically stable.
- Local Control: adding or removing a point in this method of high-order polynomial means recalculating the whole path instead of just one segment.
The method is implemented in this file in the function higher_order_poly_3pt
When provided with
The overall function is given by
By adopting this approach, it becomes essential to calculate four coefficients for each polynomial. Given that
- The initial and final velocities
- The initial and final accelerations
- The periodic conditions for velocity and acceleration
- The continuity of jerk
For given the points of
The conditions will be:
The coefficient
if we consider each velocity at time
Where
But this is for when the velocities of the points are known, which they are not (except the initial and final points). So the velocities have to be calculated, in this instance we use the continuity conditions of acceleration:
Velocities can be found with a matrix of
Thus we have the velocities and the problem is solved. (for more details go to reference [10] Chapter 4.4). The implementation of this problem is coded in this python file and the results are:
first method: intial and final zero acceleration
for more control over the start and finishing points we can use 4-th order polynomial for the start and finishing points. so instead of using n polynomials with an order of 3, we'll use n-2 polynomials with order of 3 and two polynmials with an order of 4. the first and final polynomials so to speak. This will give us two more constants hence, we can apply two more constraints.
second method: smooth acceleration curve
Another way of imporvemnt is to use p=4 altogether. this requires that the calculations be re-done in a similar manner to the previous section. of course one of the polynomials have to be p=5. either first or last polynomial so to speak. because the number of constrains we want for
-
$2n$ constraints for the positions -
$n-1$ constraints for the velocity continuity -
$n-1$ constraints for the acceleration continuity -
$n-1$ constraints for the jerk continuity -
$2$ constraints for initial and final velocity -
$2$ constraints for initial and final acceleration
that adds up to
First Method: Setting initial and final acceleration to zero Second Method: Continous Jerk Profile
We talked about the trapezoidal method in one of the point-to-point methods, but now we want to use it as a multi-point method. we already know about the 3 phases in trapezoidal. Assume that we want to use point-to-point interpolation on multiple points. What's the problem with that? it's the fact that the end effector will stop at all of the points that we want to hit. meaning if we define our points as
But since
I wish it was as easy as this though. since we have three motors we have to synchronize them first and then we can generate the velocity profile.
3D Animation for results of the sampled data of generated trajectories.
4567.mp4
cubic.spline.mp4
In this section I'm going down a more practical approach. The following is the DPR I'll be working with at the TaarLab
In the following Link You can find the Python code for communicating with the drivers.
DISCLAIMER: I did not write most of the following code as it was provided for me, I'm merely writing a documentation for the code and modify slightly if necessary.
Python Code For Driver Communication
Simple PID Controller
PID.-.circle.motion.mp4
Point to Point: 3-4-5 Interpolation Polynomial (With PID Controller)
Point to Point: 4-5-6-7 Interpolation Polynomial (With PID Controller)
4567.movement.mp4
Point to Point: Trapezoidal (With PID Controller)
Multi-Point: Higher Order Polynomials
Multi-Point: Cubic Spline
Joy-Stick Mode
[1] doi: /10.1007/978-3-030-03538-9 23
[2] doi: 10.32629/jai.v5i1.505
[3] doi: 10.1109/CRC.2017.38
[4] doi: 10.3390/app9214491
[5] doi: 10.1007/s00170-019-04421-7
[6] doi: 10.3390/app12168145
[7] Research of Trajectory Planning for Delta Parallel Robots, 2013 International Conference on Mechatronic Sciences, Electric Engineering and Computer (MEC)
[8] doi: 10.1007/s11786-012-0123-8
[9] Fundamentals of Robotic Mechanical Systems Theory, Methods, and Algorithms, Fourth Edition by Jorge Angeles
[10] Trajectory Planning for Automatic Machines and Robots by Luigi Biagiotti, Claudio Melchiorri