Loop closure equation and Newton-Raphson method

Loop-closure equation

Represent the links as vectors $\vec r_1$ (frame), $\vec r_2$ (crank), $\vec r_3$ (coupler), $\vec r_4$ (follower) with angles $\theta_1 = 0, \theta_2, \theta_3, \theta_4$ measured from the frame. The closed loop gives

$$\vec r_2 + \vec r_3 - \vec r_4 - \vec r_1 = 0$$

Splitting into components:

$$\begin{aligned} r_2\cos\theta_2 + r_3\cos\theta_3 - r_4\cos\theta_4 - r_1 &= 0 \\ r_2\sin\theta_2 + r_3\sin\theta_3 - r_4\sin\theta_4 &= 0 \end{aligned}$$

These are two equations in the two unknowns $\theta_3$ and $\theta_4$ for a given input $\theta_2$.

Freudenstein's equation (closed form)

Isolate the $r_3$ terms, square both equations and add them to eliminate $\theta_3$. Dividing by $2r_2r_4$ gives

$$K_1\cos\theta_4 - K_2\cos\theta_2 + K_3 = \cos(\theta_2 - \theta_4)$$

with $K_1 = r_1/r_2$, $K_2 = r_1/r_4$, $K_3 = \dfrac{r_1^2 + r_2^2 + r_4^2 - r_3^2}{2r_2r_4}$.

(Check: for $r_1 = 300$, $r_2 = 100$, $r_3 = 250$, $r_4 = 200$ mm at $\theta_2 = 60^\circ$, $\theta_4 = 97.56^\circ$: $K_1 = 3$, $K_2 = 1.5$, $K_3 = 1.9375$, and the left side is $0.793 = \cos(-37.56^\circ)$.)

Using the half-angle substitution $t = \tan(\theta_4/2)$ gives a quadratic $A t^2 + Bt + C = 0$, whose two roots are the open and crossed configurations. Then $\theta_3$ follows from the component equations.

Newton-Raphson iteration

Write the two component equations as $f_1(\theta_3,\theta_4) = 0$ and $f_2(\theta_3,\theta_4) = 0$.

1. Guess $\theta_3^{(0)}$ and $\theta_4^{(0)}$ (from a sketch or the previous crank position).
2. Form the Jacobian

$$J = \begin{bmatrix} -r_3\sin\theta_3 & r_4\sin\theta_4 \\ r_3\cos\theta_3 & -r_4\cos\theta_4 \end{bmatrix}$$

3. Solve $J\,\Delta = -\begin{bmatrix} f_1 \\ f_2 \end{bmatrix}$ for the corrections $\Delta = [\Delta\theta_3,\ \Delta\theta_4]^T$.
4. Update $\theta_3 \leftarrow \theta_3 + \Delta\theta_3$, $\theta_4 \leftarrow \theta_4 + \Delta\theta_4$.
5. Repeat until $|f_1|$ and $|f_2|$ are below a tolerance (about $10^{-6}$); it usually converges in 3 to 5 steps.

Comparison

The iterative method is preferred in computer programs because the previous solution gives an excellent starting guess for the next crank angle, and the same Jacobian is reused for velocity analysis.