Feedback control of two-link planar manipulator

Question

TL;DR: how can I calculate the disturbance at each joint due to coupling forces in a two-link planar robot manipulator actuated by two independent DC motors?

---

I'm studying control theory and trying to work through a simple example using a two-link planar robot manipulator:

My goal is to simulate PID control of the planar two-link manipulator, where each joint is actuated by an independent DC motor. The input is two continuous signals $$\theta_1(t), \theta_2(t)$$

which represent the desired angle for each joint at time t.

Following this paper: Modeling a Controller for an Articulated Robotic Arm, I can obtain an expression for the voltage I need to apply to each motor to achieve a desired angle. The paper also describes the PID controller necessary to maintain the desired angle given an error signal $$e(t) = \theta_{desired}(t)-\theta_{actual}(t)$$

Since each motor is controlled independently, coupling effects among joints due to varying configurations during motion are treated as disturbance inputs. My question is: how can I model this coupling effect in order to "simulate" the error signal e(t) for system under ideal conditions? By ideal conditions I mean that the disturbance due to coupling among joints accounts for 100% of the error signal.

My current thought is to use an expression for the dynamics of the two-link planar manipulator, as per the following book: A Mathematical Introduction to Robotic Manipulation

This way, at time t, we determine the voltage to achieve the desired angles for each independent motor, then plug that into our DC motor model to obtain the generated torques: $$\tau_1, \tau_2$$

We then plug these torques into the dynamics to get the actual angles, taking into account the coupling forces, and then use the actual angles compared to the desired angles in order to generate the error signal that feeds into the PID control loop.

Does this approach make sense? If not, where have I gone wrong and how can I simulate the error signal due to coupling forces?

Edit: one commenter points out PID control may not be optimal for this problem. If this is the case, what alternative control strategies should I use?

Answer 1

Convert the nonlinear model to state-space form, $x'=f(x,u)$, and linearize it to get a linear state-space or transfer function representation. You can use this to design a PID controller and simulate it that together with the original nonlinear model to see how the controller performs.

As noted, the performance of the linear controller will most likely deteriorate far from the operating point. You can design a tracking controller based on feedback linearization. This will give you the control action based on the current configuration and desired trajectory. One downside is that it will not be that robust if there are large variations in the parameters (masses, lengths). The theory for this is in the book by Isidori (link).

All this will be tedious algebraic manipulation. You can find several worked examples of these computations being performed using Mathematica. (link, link, link)

Update: Here is my attempt with some arbitrary numerical values and sinusoid reference inputs. I have also made the assumption that the masses are concentrated at the end of the links. (If you also want to include the inertia and assume that the mass is uniformly distributed, just update the Lagrangian suitably.)

Answer 2

Non-Linear Control Approach

Your approach is correct! I just want to point it out some tips.

$1^{st}$: Try to get the canonical form for the generalized coordinates:

$M(q) \ddot{q}$ + $C(q,\dot{q}) \dot{q}$ + $g(q)$ = $u$

Were $M(q)$ is the inertia matrix, $C(q,\dot{q})$ is the Coriolis matrix, $g(q)$ is the gravitational vector and, $u$, is your input for the controller.

$2^{nd}$: The goal is to make the resulting dynamics of the system an double integrator:

$a_{q} = \ddot{q}$

Note that the resultant dynamic is decoupled!

$3^{rd}$: Using the set of equations:

$M(q) \ddot{q}$ + $C(q,\dot{q}) \dot{q}$ + $g(q)$ = $u$

$u$ = $M(q) a_{q}$ + $C(q,\dot{q}) \dot{q}$ + $g(q)$

$a_{q} = \ddot{q}$

You just gonna need to choose the variable $a_{q}$ for the controller. (Substitute those $u$'s to verify the double integrator)

$4^{rd}$: Defining the error as $e = q - q_{d}$, were $q_{d}$ is your desired joint position.

$\ddot{e} + K_{1}\dot{e} + K_{0}e$ = 0

The explicit form:

$a_{q} = \ddot{q}_{d} - K_{1} \big( \dot{q}- \dot{q}_{d} \big) + K_{0} \big( {q}-{q}_{d} \big) $

If your matrices $K_{0}$ and $K_{1}$ are symmetric and positive-defined, the error dynamic is asymptotically stable.

To see more, I suggest the reading of this book. ( The figure is from there ).