# 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.

The motion of such a robot is a big challenge and needs a bunch of considerations. In this case the motion of all the joints should be determined considering the position, velocity and acceleration constraints. A poor motion planning obviously yields undesired behavior which might cause the robot to fail to perform its tasks. Here we are to address some of the challenges in robot motions and the solutions.
First let’s take a look at different components which are used in industry to perform motion planning tasks.

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 So the position (angle) of the joint is determined by this equation at each point of time.
A higher degree polynomial yields a more precise or in other words smoother trajectory, but on the other hand, it is more complicated to solve.

### 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. Solving the equation above for the desired conditions, results in constants as function of the boundary conditions, so a unique trajectory equation is achieved. Now we have all the parameters and can have our specific trajectory equation. The following pictures show a trajectory diagrams for these specified boundary conditions:    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

As we saw in cubic trajectory, it can satisfy up to 4 boundary conditions. So if we want to have acceleration conditions for the boundaries, we need a higher-order polynomial.
By adding two boundary conditions for starting and finishing accelerations, we will have six boundary conditions. A fifth-degree polynomial can satisfy these six conditions for us. In order to calculate the constants, we impose the boundary conditions. By substituting t_f – t_i with T, we obtain the following equations: In the following Images you can see an example of quintic polynomials that has these boundary conditions:    You can see that acceleration is continuous in this trajectory.

### 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. You can see diagrams of harmonic trajectory with specified conditions:    In this trajectory condition acceleration is discontinuous at both initial and final conditions.

### Cycloidal Trajectory

In harmonic trajectory the acceleration in boundaries has discontinuity. This sudden change in acceleration causes big amounts of jerk. In order to avoid the issue we can use cycloidal trajectory. In this trajectory the initial and final acceleration are considered in boundary conditions and are selected so that there is no discontinuity. And then we have an example with these boundary conditions.    ### 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  You can see an example of this trajectory for the following conditions the pictures below   ## 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   In this trajectory, the acceleration reaches +30 deg/sec^2 between 1s and 2s, and reaches -30 deg/sec^2 between 5s and 6s. During the motion, there is a discontinuity in the acceleration which limits the amount of jerk.

## 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

In this method, we choose ‘p’ to be three. This means we split the path to ‘n’ section and use a cubic trajectory for each section. Now for a trajectory which has n points along the path, we should reach to ‘n’ polynomials that each one is of order ‘3’.
When we have just the intermediate points as our input, we should determine the intermediate velocities ourselves. We use the following rules to determine velocity at intermediate points. So at the initial and final point, the velocity is zero. At intermediate points, the velocity is equal to the average velocity before and after the point. The speed between every two points (k-1 and k) is calculated by the following formula. The big disadvantage of this method is the discontinuity of acceleration at intermediate points.

## Interpolation with Spline

As you saw in the last section, when interpolation with qubics, there is discontinuity of acceleration at intermediate points. Consequently we should change the boundary conditions at intermediate points in such a way that speed and velocity are continuous. This trajectory method is called Spline (smooth path line).
Using Spline, we have n-1 polynomials of order 3 for n points: 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: We know that the amount of speed at intermediate points is not determined. So we should find the Vk values for K=2,…,n-1. It can be approved that the amount of velocity at the intermediate point can be calculated by the following formula. By writing the equation for all intermediate points, we have a matrix equation: 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

Another way to interpolate intermediate points is to connect them with a straight line which means constant speed between each two points.Though it is a very simple procedure, it causes discontinuity in acceleration and jerk.In order to avoid this problem, we can connect the linear segments by parabolic bends. So the resulting trajectory does not cross the intermediate points, although it gets really close to them. In this case the intermediate points are called via points.

## TVP and interpolation of points

We can use a sequence of trapezoidal velocity profiles to solve a trajectory planning problem with intermediate points. In this method, the velocity is zero at intermediate points which means the joint stops when it reaches every position. To avoid the issue, the joint can start the panning of trajectory before the preceding one ends. This means the joint does not reach the intermediate points but close to them.

## 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.