Acceleration controller for Redundant System
A redundant system has additional degrees of freedom that expand an operation while securing the performance of task execution.
Let us consider a map
h q : q ∈ R m → x ∈ R n , \begin{align}
h_{q}: \bm{\bm{q}}\in\mathbb{R}^{m} \rightarrow \bm{x}\in\mathbb{R}^{n},
\end{align} h q : q ∈ R m → x ∈ R n , where
q \bm{\bm{q}} q and
x \bm{x} x stands for generalized positions on a joint space and work space, and
m m m ,
n n n ( m > n ) (m>n) ( m > n ) denote the dimensions of the vectors.
Note that the map is non-injective.
Let a joint space and work space be a differentiable manifold.
A velocity and acceleration on a joint space map to
x ˙ = J a c o ( q ) q ˙ x ¨ = J a c o ( q ) q ¨ + J ˙ a c o ( q ) q ˙ , \begin{align}
\dot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{\bm{q}})\dot{\bm{\bm{q}}} \\
\ddot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{\bm{q}})\ddot{\bm{\bm{q}}} + \dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{\bm{q}}},
\end{align} x ˙ x ¨ = J aco ( q ) q ˙ = J aco ( q ) q ¨ + J ˙ aco ( q ) q ˙ , where
J a c o \bm{J}_{\rm aco} J aco denotes a Jacobian matrix
J a c o ( q ) ≡ ∂ h q ( q ) ∂ q ∈ R n × m . \begin{align}
\bm{J}_{\rm aco}(\bm{\bm{q}}) \equiv \frac{\partial h_{q}(\bm{\bm{q}})}{\partial \bm{\bm{q}}} \in \mathbb{R}^{n \times m}.
\end{align} J aco ( q ) ≡ ∂ q ∂ h q ( q ) ∈ R n × m . When
x ¨ \ddot{\bm{x}} x ¨ is given,
q ¨ \ddot{\bm{q}} q ¨ cannot be sought uniquely.
Solution candidates are
q ¨ = J a c o + ( q ) ( x ¨ − J ˙ a c o ( q ) q ˙ ) + J a c o n u l l ( q ) χ , \begin{align}
\ddot{\bm{q}}&=\bm{J}_{\rm aco}^{+}(\bm{q})\left(\ddot{\bm{x}}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\chi},
\end{align} q ¨ = J aco + ( q ) ( x ¨ − J ˙ aco ( q ) q ˙ ) + J aco null ( q ) χ , where
J a c o + ( q ) ≡ J a c o T ( q ) ( J a c o ( q ) J a c o T ( q ) ) − 1 J a c o n u l l ( q ) = ( I − J a c o + ( q ) J a c o ( q ) ) . \begin{align}
\bm{J}_{\rm aco}^{+}(\bm{q})&\equiv \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{J}_{\rm aco}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\right)^{-1} \\
\bm{J}_{\rm aco}^{\rm null}(\bm{q}) &= \left(\bm{I}-\bm{J}_{\rm aco}^{+}(\bm{q})\bm{J}_{\rm aco}(\bm{q})\right).
\end{align} J aco + ( q ) J aco null ( q ) ≡ J aco T ( q ) ( J aco ( q ) J aco T ( q ) ) − 1 = ( I − J aco + ( q ) J aco ( q ) ) . The second term of the solution candidates is the null space of the map,
J a c o n u l l ( q ) χ ∈ K e r ( J a c o ( q ) ) , \begin{align}
\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\chi} \in {\rm Ker}(\bm{J}_{\rm aco}(\bm{q})),
\end{align} J aco null ( q ) χ ∈ Ker ( J aco ( q )) , where
J a c o n u l l ( q ) \bm{J}_{\rm aco}^{\rm null}(\bm{q}) J aco null ( q ) is orthogonal projection to the nul space.
A joint-space acceleration in the null space does not affect
x ¨ \ddot{\bm{x}} x ¨ but an internal system, called redundant degrees of freedom.
Here, let
J \bm{J} J and
M \bm{M} M be an inertia of joint-space system and work-space system.
Since a kinetic energy of a system is constant on any coordinates, an equation
1 2 q ˙ T J ( q ) q ˙ = 1 2 x ˙ T M ( q ) x ˙ . \begin{align}
\frac{1}{2}\dot{\bm{q}}^{\mathrm T}\bm{J}(\bm{\bm{q}})\dot{\bm{q}} &= \frac{1}{2}\dot{\bm{x}}^{\mathrm T}\bm{M}(\bm{\bm{q}})\dot{\bm{x}}.
\end{align} 2 1 q ˙ T J ( q ) q ˙ = 2 1 x ˙ T M ( q ) x ˙ . holds, i.e.,
J ( q ) = J a c o T ( q ) M ( q ) J a c o ( q ) . \begin{align}
\bm{J}(\bm{\bm{q}}) &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\bm{J}_{\rm aco}(\bm{\bm{q}}).
\end{align} J ( q ) = J aco T ( q ) M ( q ) J aco ( q ) . It further leads
J ( q ) = J a c o T ( q ) M ( q ) J a c o ( q ) ⇔ J ( q ) J a c o + ( q ) = J a c o T ( q ) M ( q ) J a c o ( q ) J a c o + ( q ) = J a c o T ( q ) M ( q ) ⇔ J a c o + ( q ) = J − 1 ( q ) J a c o T ( q ) M ( q ) ⇔ I = J a c o ( q ) J − 1 ( q ) J a c o T ( q ) M ( q ) ∴ M − 1 ( q ) = J a c o ( q ) J − 1 ( q ) J a c o T ( q ) . \begin{align}
\bm{J}(\bm{\bm{q}}) &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\bm{J}_{\rm aco}(\bm{\bm{q}}) \\
\Leftrightarrow \bm{J}(\bm{\bm{q}}) \bm{J}_{\rm aco}^{+}(\bm{\bm{q}}) &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\bm{J}_{\rm aco}(\bm{\bm{q}}) \bm{J}_{\rm aco}^{+}(\bm{\bm{q}}) = \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\\
\Leftrightarrow \bm{J}_{\rm aco}^{+}(\bm{\bm{q}}) &= \bm{J}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\\
\Leftrightarrow \bm{I} &= \bm{J}_{\rm aco}(\bm{\bm{q}}) \bm{J}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{M}(\bm{\bm{q}})\\
\therefore \bm{M}^{-1}(\bm{q}) &= \bm{J}_{\rm aco}(\bm{\bm{q}}) \bm{J}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}}).
\end{align} J ( q ) ⇔ J ( q ) J aco + ( q ) ⇔ J aco + ( q ) ⇔ I ∴ M − 1 ( q ) = J aco T ( q ) M ( q ) J aco ( q ) = J aco T ( q ) M ( q ) J aco ( q ) J aco + ( q ) = J aco T ( q ) M ( q ) = J − 1 ( q ) J aco T ( q ) M ( q ) = J aco ( q ) J − 1 ( q ) J aco T ( q ) M ( q ) = J aco ( q ) J − 1 ( q ) J aco T ( q ) . Motion equations on a joint space and work space are described as
τ = J ( q ) q ¨ f = M ( q ) x ¨ . \begin{align}
\bm{\tau}&=\bm{J}(\bm{\bm{q}})\ddot{\bm{q}}\\
\bm{f}&=\bm{M}(\bm{\bm{q}})\ddot{\bm{x}}.
\end{align} τ f = J ( q ) q ¨ = M ( q ) x ¨ . Here, we can found
J a c o T ( q ) f = J a c o T ( q ) M ( q ) J ˙ a c o ( q ) q ˙ + J a c o T ( q ) M ( q ) J a c o ( q ) q ¨ = J a c o T ( q ) M ( q ) J ˙ a c o ( q ) q ˙ + J ( q ) q ¨ = J a c o T ( q ) M ( q ) J ˙ a c o ( q ) q ˙ + τ \begin{align}
\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{f}
&=\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}+\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{M}(\bm{\bm{q}})\bm{J}_{\rm aco}(\bm{q})\ddot{\bm{q}}\\
&=\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}+\bm{J}(\bm{q})\ddot{\bm{q}}\\
&=\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}+\bm{\tau}\\
\end{align} J aco T ( q ) f = J aco T ( q ) M ( q ) J ˙ aco ( q ) q ˙ + J aco T ( q ) M ( q ) J aco ( q ) q ¨ = J aco T ( q ) M ( q ) J ˙ aco ( q ) q ˙ + J ( q ) q ¨ = J aco T ( q ) M ( q ) J ˙ aco ( q ) q ˙ + τ Therefore, we found a particular solution
τ = J a c o T ( q ) ( f − M ( q ) J ˙ a c o ( q ) q ˙ ) . \begin{align}
\bm{\tau} &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\left(\bm{f}-\bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right).
\end{align} τ = J aco T ( q ) ( f − M ( q ) J ˙ aco ( q ) q ˙ ) . Since the system has the null space, a general solution is given as
τ = J a c o T ( q ) ( f − M ( q ) J ˙ a c o ( q ) q ˙ ) + J ( q ) J a c o n u l l ( q ) ξ . \begin{align}
\bm{\tau}&=\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{f}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+\bm{J}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi}.
\end{align} τ = J aco T ( q ) ( f − M ( q ) J ˙ aco ( q ) q ˙ ) + J ( q ) J aco null ( q ) ξ . Work space acceleration control
Acceleration control is achieved when
M ( q ) = I . \begin{align}
\bm{M}(\bm{q}) = \bm{I}.
\end{align} M ( q ) = I . It is achieved by controlling a virtual inertia of a joint-space system using a joint-space acceleration controller.
A joint-space acceleration controller makes a virtual inertia
J v ( q ) = I . \begin{align}
\bm{J}_{\rm v}(\bm{q}) = \bm{I}.
\end{align} J v ( q ) = I . By controlling this value, a work-space acceleration control is indirectly achieved.
For this purpose, the following condition should hold:
J a c o ( q ) J v − 1 ( q ) J a c o T ( q ) = I ⇔ J v − 1 ( q ) J a c o T ( q ) = J a c o + ( q ) . \begin{align}
&\bm{J}_{\rm aco}(\bm{\bm{q}}) \bm{J}_{\rm v}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}}) = \bm{I}\\
\Leftrightarrow &\bm{J}_{\rm v}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}}) = \bm{J}^{+}_{\rm aco}(\bm{q}).
\end{align} ⇔ J aco ( q ) J v − 1 ( q ) J aco T ( q ) = I J v − 1 ( q ) J aco T ( q ) = J aco + ( q ) . For implementation, a control input to be calculated is a joint-space torque.
Assuming that a joint-space acceleration controller is implemented, we show a calculation process of an acceleration reference.
When work-space force reference
f r e f \bm{f}^{\rm ref} f ref is given, a joint-space torque reference is calculated as
τ r e f = J a c o T ( q ) ( f r e f − M ( q ) J ˙ a c o ( q ) q ˙ ) + J v ( q ) J a c o n u l l ( q ) ξ = J a c o T ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) + J v ( q ) J a c o n u l l ( q ) ξ . \begin{align}
\bm{\tau}^{\rm ref} &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right) +\bm{J}_{\rm v}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi}\\
&=\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right)+\bm{J}_{\rm v}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi}.
\end{align} τ ref = J aco T ( q ) ( f ref − M ( q ) J ˙ aco ( q ) q ˙ ) + J v ( q ) J aco null ( q ) ξ = J aco T ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) + J v ( q ) J aco null ( q ) ξ . Considering a virtual joint-space inertia, an acceleration reference is caluclated as
q ¨ r e f = J v − 1 τ r e f = J v − 1 ( q ) J a c o T ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) + J v − 1 ( q ) J v ( q ) J a c o n u l l ( q ) ξ = J a c o + ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) + J a c o n u l l ( q ) ξ \begin{align}
\ddot{\bm{q}}^{\rm ref} &= \bm{J}_{\rm v}^{-1}\bm{\tau}^{\rm ref}\\
&=\bm{J}_{\rm v}^{-1}(\bm{\bm{q}})\bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right)+\bm{J}_{\rm v}^{-1}(\bm{\bm{q}})\bm{J}_{\rm v}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi}\\
&=\bm{J}_{\rm aco}^{+}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right)+\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi}
\end{align} q ¨ ref = J v − 1 τ ref = J v − 1 ( q ) J aco T ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) + J v − 1 ( q ) J v ( q ) J aco null ( q ) ξ = J aco + ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) + J aco null ( q ) ξ Assuming that an joint-space acceleration control works,
q ¨ = q ¨ r e f \begin{align}
\ddot{\bm{q}} &= \ddot{\bm{q}}^{\rm ref}
\end{align} q ¨ = q ¨ ref holds. Then, a work-space acceleration is calculated as
x ¨ = J a c o ( q ) q ¨ r e f + J ˙ a c o ( q ) q ˙ = J a c o ( q ) J a c o + ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) + J a c o ( q ) J a c o n u l l ( q ) ξ + J ˙ a c o ( q ) q ˙ = f r e f − J ˙ a c o ( q ) q ˙ + J ˙ a c o ( q ) q ˙ = f r e f . \begin{align}
\ddot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{\bm{q}})\ddot{\bm{\bm{q}}}^{\rm ref} + \dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{\bm{q}}}\\
&= \bm{J}_{\rm aco}(\bm{\bm{q}})\bm{J}_{\rm aco}^{+}(\bm{q})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right) + \bm{J}_{\rm aco}(\bm{\bm{q}})\bm{J}^{\rm null}_{\rm aco}(\bm{\bm{q}})\bm{\xi} + \dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{\bm{q}}}\\
&= \bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}} + \dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{\bm{q}}}\\
&= \bm{f}^{\rm ref}.
\end{align} x ¨ = J aco ( q ) q ¨ ref + J ˙ aco ( q ) q ˙ = J aco ( q ) J aco + ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) + J aco ( q ) J aco null ( q ) ξ + J ˙ aco ( q ) q ˙ = f ref − J ˙ aco ( q ) q ˙ + J ˙ aco ( q ) q ˙ = f ref .