Motion Planning In Servo Drives

Introduction

In the robotic industry it is always necessary to have moving parts. The moving process can be of different types. Though some are as simple as moving a pneumatic cylinder back and forth, in some cases it can be more challenging. Consider a three joint robotic arm which is in charge of picking up an object and putting it somewhere else.

Image references here.

A servomechanism is used to perform highly accurate motions in the robotic and industrial automation industry. A big step to achieve the goal, is to have a plane for the motion that the servo must do. This plan is designed by a machine or robot designer and programmer and is given to the controller. To get a perspective of what motion planning is, consider a robotic arm. The arm’s end effector moves from one point to another. The end actuator moves along a path in space to reach the point to the desired destination. So here we have some definitions which characterizes the motions:

Path: Is the trajectory of the line of the motion of the arm. This line can be determined in different spaces like the space of cartesian positions, the space of the orientations, the joint space, etc.
Timing Law: The time which the arm should move along its path is called timing law.
Trajectory: When the timing law is assigned to a path, we have the trajectory. In other words, trajectory determines where the arm should be at any time.

So when you solve a motion planning problem, at the end you have a trajectory which is given to the arm controller to make it move.

Operational Space: Basically in a robot, you are interested in the movement of the end effector. For example it should move from point A to point B along a specific path. The operational (or task) space is the convenient cartesian space which can be used to specify the path and orientation of the end effector.
Joint Space: The robot you are about to solve the trajectory for, consists of some joints which in collaboration to each other move the end effector. These joints are mostly servos. If you solve the trajectory using every joint position (angle) and maybe speed and acceleration, you are solving in joint space.

Every trajectory planner has some inputs and outputs. Actually inputs are the space and the points in it that you want your robot moves according to. So we can say we have three inputs for a trajectory planner which are:
1 – Space
2 – Path
3 – Timing law

The output of the trajectory planner is a point sequence in the relevant space. These points are calculated in a fix rate for like 10ms or 20ms. Based on the controller, these points can be interpolated so we have a bunch of new points between the points generated by the trajectory planning solution.

When we deal with a trajectory planning problem in joint space, we want to achieve an equation q(t) that defines the relation between position, i.e. rotation of a servo motor, and time. The q(t) can be any arbitrary function, as long as it satisfies our conditions. So our method and desired conditions determine the characteristics of q(t).

Polynomial Trajectories

The first type of trajectory method we are going to discuss is called “Polynomial Trajectory”.  As its name implies, the q(t) is a polynomial which in the general form is as follows

Cubic Polynomials

In this method, q(t) is a third-degree polynomial, so it can satisfy up to four boundary conditions. These boundary conditions are the initial and final position and velocity.

As you can see, the acceleration is not continuous in this trajectory at the initial and final points. This is a disadvantage for this trajectory and to solve it, we need to determine boundary conditions for acceleration which is not possible using this trajectory.

Quintic Polynomials

In order to calculate the constants, we impose the boundary conditions. By substituting t_f – t_i with T, we obtain the following equations:

Harmonic Trajectory

This trajectory method uses sinusoidal functions to get to the trajectory. So the position of the joint varies sinusoidally with time. So the acceleration of the joint has 𝛑 phase shift compared to position. It means acceleration is proportional to position, with a negative sign.

Cycloidal Trajectory

Trapezoidal velocity profile (TVP)

This may be the most common practice in the industry. In this trajectory planning, there are three regions.
1- Acceleration region: In the section, the speed increases with constant acceleration until it reaches a specific value
2- Constant speed region: After reaching the desired speed, the acceleration changes to zero and the joint rotates with a constant speed.
3- Deceleration region: In the last part, the joint slows down with a constant acceleration.
So the velocity curve has a trapezoidal shape in time

S-Curve Trajectory

Acceleration in trapezoidal trajectory is not continuous which causes extremely high jerk values during the motion. In order to solve this problem, the S-Curve trajectory has been introduced. This trajectory is defined in such a way that the acceleration changes with a constant slope rather than changing suddenly. When it reaches a maximum amount of acceleration, the acceleration remains constant for a specific time period. Then acceleration starts decreasing until it is zero. Then the joint keeps rotating with a constant velocity.  At the stopping stage, the acceleration decreases with a constant slope and after getting a maximum negative value, it remains constant for a while. At the end stage, acceleration increases until it is zero. You can see a sample of the S-curve trajectory in the following images

ST-Curve Trajectory

Another type of trajectory is called ST curve trajectory. The difference between s-curve and st-curve trajectory, is that in st-curve, the jerk is also continuous during the motion which leads to smoother motion. In order to have this trajectory, acceleration should have a second order function of time while it increases or decreases during the motion.

Comparison Table

Now let’s have an overview of all the trajectories we discussed in this article.

Interpolation of Points

Until now we discussed trajectory planning problems which are dealing only with the initial and the final points of a motion. Now let’s consider more general cases which consist of a sequence of points. In these cases we are not only interested in the initial and the final points, but also we have some intermediate points which must be crossed during the motion. So we consider a total number of n points for the path of the motion. We name these points q_1 , q_2, … , q_n. These point must be reached at specific times, called t_1, t_2, … , t_n.
Therefore we have n conditions that need to be met. The equation representing the trajectory, is reached by adopting a polynomial of degree n-1.

We need to solve the equations for all the points to find the constants a_0 , a_1,…, a_n-1. We can write the equations in matrix form

The clear advantage of polynomial interpolation is that it has continuous derivatives of all the orders. But This method has big disadvantages which makes it not so useful.
1) First of all,it has a big disadvantage from a calculation efficiency point of view. To solve the equations for a_0 , a_1 , … , a_n-1, we need to calculate inverse of a n*n matrix. As the order of the matrix increases, the amount of calculations rises dramatically which is in most cases not efficient.
2) If you need to change any single point or add a new point, all the coefficients change and you need to calculate all of them again.
3) This method of trajectory might cause undesired oscillations.
Considering the mentioned disadvantages, splitting the path to a sequence of paths might be a good idea! So we break down the nth order polynomial to ‘n’ polynomials, each one of ‘p’ order, representing a trajectory for the section.

Interpolation with Cubics

The big disadvantage of this method is the discontinuity of acceleration at intermediate points.

Interpolation with Spline

So we have 4(n-1) coefficients. The conditions to be imposed are:
1.   2(n-1) conditions for the passage through the points. Since the points are determined, each cubic polynomial must satisfy the positions of its boundaries.
2.   n-1 conditions to consider velocity continuity at all intermediate points.
3.   n-1 conditions to consider acceleration continuity at all intermediate points
The sum of the above boundary conditions is equal to 4n-6. So there are two conditions left which are the residual freedom left and need to be determined deliberately. A way to use them is to specify the initial and final velocities.
So we have the following equations:

And the boundary conditions are as follows:

So we have an equation in the form of Av=C. Since the A is a dominant diagonal matrix and is always invertible we can write the equation as v=A^-1 * C. So we solved the problem.

Interpolation with linear segments

TVP and interpolation of points

Feedback system

Sensors are one the fundamental components in every control system. In other words, you can control a parameter just when you can measure it or its effects. In a motion control system, where you are dealing with controlling and adjusting position, the main parameter which should be measured is position. Is possible to read more about the topic in this article.