Resolved-motion Rate Control

Resolved-motion rate control converts a required tool-configuration velocity into joint velocities.

flowchart LR
    X["Required tool path<br/>x(t)"] --> XD["Differentiate<br/>x-dot(t)"] --> R{"Is V(q) full rank?"}
    R -- "NO" --> S["Singularity<br/>replan the path"]
    R -- "YES" --> QD["Apply V-plus(q)<br/>find q-dot(t)"]
    QD --> INT["Integrate q-dot(t)"] --> Q["Joint trajectory<br/>q(t)"]

Start with the differential kinematics relation:

$$\dot{x}=V(q)\dot{q}$$

For a differentiable trajectory $x(t)$ inside the work envelope, the trajectory must stay away from workspace singularities.

Case: $n\le 6$

When $V(q)$ has full column rank:

$$V^{+}(q)={\left[V^T(q)V(q)\right]}^{-1}{V}^T(q)$$

The joint-space trajectory is obtained from the nonlinear differential equation:

$$\boxed{\dot{q}={\left[V^T(q)V(q)\right]}^{-1}{V}^T(q)\dot{x}}$$

with the initial condition:

$$q(0)=w^{-1}(x(0))$$

The initial condition uses Inverse Kinematics to make the joint trajectory and tool trajectory start at the same physical configuration.

Case: $n=6$

When $V(q)$ is square and nonsingular:

$$\boxed{\dot{q}=V^{-1}(q)\dot{x}}$$

This operation resolves the tool motion into its joint-space components.

Four-axis SCARA example

Write the nonconstant components of the SCARA Jacobian as:

$$V(q)=\left[\begin{array}{cccc}\alpha & \beta & 0& 0\\ \gamma & \delta & 0& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& \epsilon \end{array}\right]$$

where:

$$\epsilon =-\frac{\exp (q_4/\pi )}{\pi }\ne 0$$

Define:

$$\Delta =({\alpha }^2+{\gamma }^2)({\beta }^2+{\delta }^2)-(\alpha \beta +\gamma \delta {)}^2$$

The pseudoinverse is well-defined when $\Delta \ne 0$:

$$V^{+}=\frac{1}{\Delta }\left[\begin{array}{cccccc}(\alpha \delta -\beta \gamma )\delta & (\beta \gamma -\alpha \delta )\beta & 0& 0& 0& 0\\ (\beta \gamma -\alpha \delta )\gamma & (\alpha \delta -\beta \gamma )\alpha & 0& 0& 0& 0\\ 0& 0& -\Delta & 0& 0& 0\\ 0& 0& 0& 0& 0& \Delta /\epsilon \end{array}\right]$$

The rate-control equations are:

$${\dot{q}}_1=\frac{(\alpha \delta -\beta \gamma )\delta {\dot{x}}_1+(\beta \gamma -\alpha \delta )\beta {\dot{x}}_2}{\Delta }$$ $${\dot{q}}_2=\frac{(\beta \gamma -\alpha \delta )\gamma {\dot{x}}_1+(\alpha \delta -\beta \gamma )\alpha {\dot{x}}_2}{\Delta }$$ $${\dot{q}}_3=-{\dot{x}}_3$$ $${\dot{q}}_4=\frac{{\dot{x}}_6}{\epsilon }$$

The SCARA approach vector is always vertical, so a valid tool trajectory must satisfy:

$$x_4(t)=0,x_5(t)=0$$

Important

As the robot approaches a singularity, $\Delta$ becomes small. The required values of ${\dot{q}}_1$ and ${\dot{q}}_2$ can then become very large.

See Tool-configuration Jacobian Matrix and Generalised Inverses.