Trajectory tracking for a robotic arm

Question

I want a robotic arm to do trajectory tracking in the joint space. Suppose the waypoints are $$ [t(0),q_1(0),q_2(0),q_3(0)],\\ \cdots,\\ [t(n-1),q_1(n-1),q_2(n-1),q_3(n-1)] $$ I already know that I can use these waypoints to get a smooth trajectory $[q_1(t),q_2(t),q_3(t)]$ for each joint through interpolation, but how to control the motor (current control or torque control is supported) to track the smooth trajectory properly? More specifically, how to make sure that these motors could keep in sync?

Answer 1

For simplicity let's consider a 2R planar manipulator. This method can be expanded to any robot as long as you can define error-proof dynamics. Equations of motions for this robot can be defined by the following equations.

EoM 1 :

EoM 2:

Here, u1 and u2 are torques required for joints 1 and 2 respectively.

The rest of the parameters are shown in the Image.

Note that, these equations can be derived using the lagrangian method.

These EoMs can be written in manipulator form.

The corresponding matrices are :

The desired torques to track required trajectory can be computed by :

This equation is obtained by feedback linearization . You can read more about it here. In this equation, K matrix contains PD gains. Desired theta and desired angular velocity can be computed using either cubic or quentic polynomial.

If you have a starting angle and ending angle and theie velocities at two instants , the cubic polynomial to track the trajectory would be :

You can compute the coefficients of these cubic polynomials based on your desired trajectory. If you want jerk free motion , you can add a fifth order term to your polynomial.

You can try different PD gains or you can compute them using eigen value placement. For this robot K matrix is 2X4. You need to use following state and input matrices for eigen value placement.

Results of this implementation looks as shown below.