Acceleration controller for Multi-DoF System
If an acceleration controller can be constructed on a joint space, acceleration control on another coordinate system, called a work space, is indirectly available.
Let us consider a injective map
h q : q ∈ R m ↪ x ∈ R m , \begin{align}
h_{q}: \bm{\bm{q}}\in\mathbb{R}^{m} \hookrightarrow \bm{x}\in\mathbb{R}^{m},
\end{align} h q : q ∈ R m ↪ x ∈ R m , 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 denotes the dimension of the vectors.
A point on a joint space corresponds to a point on a work space one to one, i.e., there is no redundant degree-of-freedom.
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 m × 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}^{m \times m}.
\end{align} J aco ( q ) ≡ ∂ q ∂ h q ( q ) ∈ R m × m . 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 ) M ( q ) = J a c o − T ( q ) J ( q ) J a c o − 1 ( 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}})\\
\bm{M}(\bm{\bm{q}}) &= \bm{J}_{\rm aco}^{-{\mathrm T}}(\bm{\bm{q}})\bm{J}(\bm{\bm{q}})\bm{J}_{\rm aco}^{-1}(\bm{\bm{q}}).
\end{align} J ( q ) M ( q ) = J aco T ( q ) M ( q ) J aco ( q ) = J aco − T ( q ) J ( q ) J aco − 1 ( 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, forces are mutually map to the other
τ = J a c o T ( q ) ( f − M ( q ) J ˙ a c o ( q ) q ˙ ) f = J a c o − T ( q ) τ + 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)\\
\bm{f} &= \bm{J}_{\rm aco}^{-{\mathrm T}}(\bm{\bm{q}})\bm{\tau} + \bm{M}(\bm{\bm{q}})\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}.
\end{align} τ f = J aco T ( q ) ( f − M ( q ) J ˙ aco ( q ) q ˙ ) = J aco − T ( q ) τ + M ( q ) J ˙ aco ( q ) 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 setting a virtual joint-space inertia as follow, work-space acceleration control is achieved.
J v ( q ) = J a c o T ( q ) J a c o ( q ) , \begin{align}
\bm{J}_{\rm v}(\bm{q}) = \bm{J}_{\rm aco}^{\mathrm T}(\bm{\bm{q}})\bm{J}_{\rm aco}(\bm{\bm{q}}),
\end{align} J v ( 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 a c o T ( q ) ( f r e f − J ˙ a c o ( q ) 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 aco}^{\mathrm T}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right).
\end{align} τ ref = J aco T ( q ) ( f ref − M ( q ) J ˙ aco ( q ) q ˙ ) = J aco T ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) . Considering a virtual joint-space inertia, an acceleration reference is caluclated as
q ¨ r e f = J v − 1 τ r e f = J a c o − 1 ( q ) J a c o − T ( q ) J a c o T ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) = J a c o − 1 ( q ) ( f r e f − J ˙ a c o ( q ) q ˙ ) . \begin{align}
\ddot{\bm{q}}^{\rm ref} &= \bm{J}_{\rm v}^{-1}\bm{\tau}^{\rm ref}\\
&=\bm{J}_{\rm aco}^{-1}(\bm{\bm{q}})\bm{J}_{\rm aco}^{-{\mathrm T}}(\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 aco}^{-1}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right).
\end{align} q ¨ ref = J v − 1 τ ref = J aco − 1 ( q ) J aco − T ( q ) J aco T ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) = J aco − 1 ( q ) ( f ref − J ˙ aco ( q ) 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 − 1 ( 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}^{-1}(\bm{\bm{q}})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{\bm{q}})\dot{\bm{q}}\right) + \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 − 1 ( q ) ( f ref − J ˙ aco ( q ) q ˙ ) + J ˙ aco ( q ) q ˙ = f ref .