Paper Read 6 - A Novel QP-Based Kinematic Redundancy Resolution Method With Joint Constraints Satisfaction

22 Jan 2024

I want to document the learning process of reading a paper which uses velocity-level IK method achieving joint acceleration constraints together with the velocity- and position-level constraints.

Abstract

A novel redundancy resolution method based on quadratic programming (QP) approach
Velocity-level IK method allows the joint constraints at the position, velocity, and acceleration levels.
The discretized joint state equations allow the use of joint accelerations as decision variables in the QP problem.

Introduction

Although the QP formulation of IK—it can be used to derive the pseudoinverse-based IK , this paper presents an important enhancement. The scientific novelty of this work is the proposition of a velocity-level IK method that allows the fulfilment of the joint acceleration constraints together with the velocity- and position-level constraints. The elements of the goal function are formulated in the form that uses accelerations instead of the usual velocities.

Method

Classic Method

The task space variables x ∈ R m, joint space variables q ∈ R n

\[\mathbf{x}=\mathbf{f}(\mathbf{q})\]

For a full 6 degree-of-freedom task, the vector x ∈ R 6 can consist of the end effector’s position r ∈ R 3 and orientation φ ∈ R 3 (given for example by Euler angles):

\[\mathbf{x}=\left[\begin{array}{l} \mathbf{r} \\ \boldsymbol{\phi} \end{array}\right]\] \[\mathbf{f}(\mathbf{q})=\left[\begin{array}{l} \mathbf{f}_{\mathbf{r}}(\mathbf{q}) \\ \mathbf{f}_\phi(\mathbf{q}) \end{array}\right]\]

The analytical task Jacobian matrix Ja is computed as

\[\mathbf{J}_a(\mathbf{q})=\frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}}\] \[\dot{\mathbf{x}}=\mathbf{J}_a \dot{\mathbf{q}},\]

In practice, it is often more convenient to use the geometric Jacobian J = J(q), since it describes the relationship between the joint velocities q_dot and task velocites v

\[\mathbf{v}=\left[\begin{array}{c} \dot{\mathbf{r}} \\ \omega \end{array}\right]\] \[\mathbf{v}=\mathbf{J} \dot{\mathbf{q}}\]

The difference between the two is that

\[\dot{\phi}=\frac{\partial \phi}{\partial \boldsymbol{q}} \dot{\boldsymbol{q}}=\boldsymbol{J}_\phi(\boldsymbol{q}) \dot{\boldsymbol{q}} \neq \omega\]

For Analytical Jacobian, in the Cartesian coordinate system, it yields an “angular velocity” which is the time derivative of the angle chosen to represent the end direction, expressed as:

\[\dot{\boldsymbol{x}}=\frac{d}{d t}([x, y, z, \phi, \theta, \psi])=\left[\frac{d \mathbf{x}}{d t}, \frac{d \mathbf{y}}{d t}, \frac{d \mathbf{z}}{d t}, \frac{d \phi}{d t}, \frac{d \theta}{d t}, \frac{d \psi}{d t}\right]\]

For Geometrical Jacobian, the “angular velocity” obtained is the velocity around the axis of rotation, expressed as:

\[\dot{\boldsymbol{x}}=\left[\frac{d \mathbf{x}}{d t}, \frac{d \mathbf{y}}{d t}, \frac{d \mathbf{z}}{d t}, \omega_x, \omega_y, \omega_z,\right]\]

angular velocity ω can be expressed as

\[\omega=\mathbf{T}(\phi) \dot{\boldsymbol{\phi}}\]

where T(φ) is a transformation matrix that corresponds to the used method of parameterization of orientation.

R(J) ⊆ R m is the range space of J(q), and it contains the end effector task velocities v that can be generated by the joint velocities q_dot for a given configuration q

N(J) ⊆ R n is the null space of J(q), and it contains the joint velocities q_dot that do not produce any task velocity, for a given configuration q

\[\begin{aligned} & \operatorname{dim}(\mathcal{R}(\mathbf{J}))=\rho \\ & \operatorname{dim}(\mathcal{N}(\mathbf{J}))=n-\rho \end{aligned}\]

For redundant manipulators, the Jacobian J(q) is not square (because m < n), Moore-Penrose pseudoinverse

\[\dot{\mathbf{q}}=\mathbf{J}^{\#} \mathbf{v}\]

In particular, the ability of the redundant manipulator to perform self motions— enabled by the existence of N(J)—might be utilized to achieve additional objectives along the main task, given by v. Therefore, the minimum norm solution can be expanded to the following form

\[\dot{\mathbf{q}}=\mathbf{J}^{\#} \mathbf{v}+\dot{\mathbf{q}}^{N S}\]

where $\dot{\mathbf{q}}^{N S} \in \mathcal{N}(\mathbf{J})$ is the null space velocity. In other words, $\dot{\mathbf{q}}^{N S}$ does not affect the end effector task velocity v, since:

\[\mathbf{J} \dot{\mathbf{q}}^{N S}=\mathbf{0}\]

The null space velocity is expressed:

\[\dot{\mathbf{q}}^{N S}=\mathbf{P} \dot{\mathbf{q}}^{J S}\]

where $\dot{\mathbf{q}}^{J S}$ is an arbitrarily chosen vector that represents the constraint of the additional task specified directly in the joint space, and $\mathbf{P}$ is the matrix which projects the vector $\dot{\mathbf{q}}^{J S}$ onto the null space of the Jacobian $\mathcal{N}(\mathbf{J})$:

\[\mathbf{P}=\mathbf{I}_n-\mathbf{J}^{\#} \mathbf{J}\]

and $\mathbf{J P}=\mathbf{0}$.

Learn from 【IIT腿足机器人控制讲义】A6:冗余机械臂(奇异，零空间优化) - 知乎 (zhihu.com) and Null Space 及 NSP-WBC基础 - 知乎 (zhihu.com)

\[\dot{\mathbf{q}}=\mathbf{J}^{\#} \mathbf{v}+\left(\mathbf{I}-\mathbf{J}^{\#} \mathbf{J}\right) \dot{\mathbf{q}}^{J S}\]

which is the solution to the inverse kinematics problem, known as the redundancy resolution at the velocity level.

One of the methods to select the vector $\dot{\mathbf{q}}^{J S}$ for the solution is to employ the local optimization:

\[\dot{\mathbf{q}}^{J S}=k_H \nabla_{\mathbf{q}} H(\mathbf{q})\] \[\mathbf{H}(\boldsymbol{q})=\frac{1}{2} \sum_{i=1}^N\left(\frac{q_i-q_{i, \mathrm{mid}}}{q_{i, \max }-q_{i, \min }}\right)^2\]

where H(q) is a scalar configuration-dependent objective function, ∇qH(q) is its gradient and kH is a scalar gain coefficient.

Specifying multiple additional tasks alongside the main one might result in conflicting task situations. Therefore, a task priority strategy is needed to ensure that each lower priority task is satisfied only in the null space of the higher priority tasks.

The multiple tasks are usually defined as:

\[\mathbf{v}^i=\mathbf{J}^i \dot{\mathbf{q}}, \quad i=1, \ldots, l\]

where vi ∈ R mi is the i-th task velocity, Ji ∈ R mi×n is the i-th task Jacobian, l is the number of tasks, mi is the i-th task dimension. The order of task priority is decreasing, i.e., the task i + 1 has a lower priority than the task i.

A recursive solution:

\[\dot{\mathbf{q}}^i=\dot{\mathbf{q}}^{i-1}+\left(\mathbf{J}^i \mathbf{P}_A^{i-1}\right)^{\#}\left(\mathbf{v}^i-\mathbf{J}^i \dot{\mathbf{q}}^{i-1}\right)\]

for $i=1, \ldots, l$, and where $\dot{\mathbf{q}}^l$ is the solution accounting for all the tasks, while $\dot{\mathbf{q}}^0=\mathbf{0}$ and $\mathbf{P}_A^0=\mathbf{I}$. The matrix $\mathbf{P}_A^i$:

\[\mathbf{P}_A^i=\mathbf{I}-\mathbf{J}_A^i{ }^{\#} \mathbf{J}_A^i=\mathbf{P}_A^{i-1}-\left(\mathbf{J}^i \mathbf{P}_A^{i-1}\right)^{\#} \mathbf{J}^i \mathbf{P}_A^{i-1}\]

is the projector onto the null space of the augmented Jacobian of the first i tasks:

\[\mathbf{J}_A^i=\left[\begin{array}{c} \mathbf{J}^1 \\ \mathbf{J}^2 \\ \vdots \\ \mathbf{J}^i \end{array}\right]\]

To obtain the joint coordinates in any given moment t, the solution has to be integrated over time (starting from t0):

\[\mathbf{q}(t)=\mathbf{q}\left(t_0\right)+\int_{t_0}^t \dot{\mathbf{q}}(\tau) d \tau\]

Equations in discrete form:

\[\mathbf{q}_{k+1}=\mathbf{q}_0+\sum_{h=0}^k \dot{\mathbf{q}}_h \Delta t \\ \mathbf{q}_{k+1}=\mathbf{q}_k+\dot{\mathbf{q}}_k \Delta t\]

The joint velocity $\dot{\mathbf{q}}_k$ is just a discrete form:

\[\dot{\mathbf{q}}_k=\mathbf{J}^{\#}\left(\mathbf{q}_k\right) \mathbf{v}_k+\mathbf{P}\left(\mathbf{q}_k\right) \dot{\mathbf{q}}_k^{J S}\] \[\mathbf{P}\left(\mathbf{q}_k\right)=\mathbf{I}-\mathbf{J}^{\#}\left(\mathbf{q}_k\right) \mathbf{J}\left(\mathbf{q}_k\right)\]

Proposed Method

JOINT STATE EQUATIONS

To directly bound the acceleration q_ddot at each time tk , the joint state equations have to be formulated at the acceleration level:

\[\left\{\begin{array}{l} \mathbf{q}_{k+1}=\mathbf{q}_k+\dot{\mathbf{q}}_k \Delta t+\frac{1}{2} \ddot{\mathbf{q}}_k(\Delta t)^2 \\ \dot{\mathbf{q}}_{k+1}=\dot{\mathbf{q}}_k+\ddot{\mathbf{q}}_k \Delta t \end{array}\right.\]

i.e.

\[\mathbf{z}_{k+1}=\mathbf{A} \mathbf{z}_k+\mathbf{B} \mathbf{u}_k\]

where the state $\mathbf{z}_k \in \mathbb{R}^{2 n}$ is:

\[\mathbf{z}_k=\left[\begin{array}{c} \mathbf{q}_k \\ \dot{\mathbf{q}}_k \end{array}\right]\]

joint acceleration is the control vector: $\mathbf{u}_k=\ddot{\mathbf{q}}_k$.

\[\begin{aligned} & \mathbf{A}=\left[\begin{array}{cc} \mathbf{I}_n & \Delta t \mathbf{I}_n \\ \mathbf{0}_{n \times n} & \mathbf{I}_n \end{array}\right] \\ & \mathbf{B}=\left[\begin{array}{c} \frac{1}{2}(\Delta t)^2 \mathbf{I}_n \\ \Delta t \mathbf{I}_n \end{array}\right] \end{aligned}\]

divided into two parts:

\[\left\{\begin{array}{l} \mathbf{q}_{k+1}=\mathbf{A}_0 \mathbf{z}_k+\mathbf{B}_0 \mathbf{u}_k \\ \dot{\mathbf{q}}_{k+1}=\mathbf{A}_1 \mathbf{z}_k+\mathbf{B}_1 \mathbf{u}_k \end{array}\right.\] \[\begin{aligned} & \mathbf{A}_0=\left[\begin{array}{ll} \mathbf{I} & \mathbf{0} \end{array}\right] \mathbf{A} \\ & \mathbf{B}_0=\left[\begin{array}{ll} \mathbf{I} & \mathbf{0} \end{array}\right] \mathbf{B} \\ & \mathbf{A}_1=\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \mathbf{A} \\ & \mathbf{B}_1=\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \mathbf{B} \end{aligned}\]

and $\mathbf{I} \in \mathbb{R}^{n \times n}$ and $\mathbf{0} \in \mathbb{R}^{n \times n}$.

The relationship between the joint and end effector velocity vectors

\[\mathbf{J}\left(\mathbf{q}_{k+1}\right) \cdot \dot{\mathbf{q}}_{k+1}=\mathbf{v}_{k+1}\]

Combining with qk, qk_dot, the following result is obtained:

\[\mathbf{J}_{k+1} \cdot\left(\mathbf{A}_1 \mathbf{z}_k+\mathbf{B}_1 \mathbf{u}_k\right)=\mathbf{v}_{k+1}\]

where:

\[\mathbf{J}_{k+1}=\mathbf{J}\left(\mathbf{q}_{k+1}\right)=\mathbf{J}\left(\mathbf{A}_0 \mathbf{z}_k+\mathbf{B}_0 \mathbf{u}_k\right)\]

An approximation of Jk+1 based on the solution from the previous step can be used to formulate a linear kinematic constraint.

an approximation $\hat{\mathbf{q}}_{k+1}$ of the future joint position $\mathbf{q}_{k+1}$ can be computed as:

\[\hat{\mathbf{q}}_{k+1}=\mathbf{A}_0 \mathbf{z}_k+\mathbf{B}_0 \hat{\mathbf{u}}_k,\]

where $\hat{\mathbf{u}}_k=\mathbf{u}_{k-1}^*$ is the solution from the previous step.

\[\hat{\mathbf{J}}_{k+1}=\mathbf{J}\left(\hat{\mathbf{q}}_{k+1}\right) .\]

a linear equation in terms of uk:

\[\hat{\mathbf{J}}_{k+1} \cdot\left(\mathbf{A}_1 \mathbf{z}_k+\mathbf{B}_1 \mathbf{u}_k\right)=\mathbf{v}_{k+1} .\]

JOINT CONSTRAINTS

The novel IK formulation requires also the inequality constraints to represent the joint position, velocity and acceleration limits.

\[\mathbf{q}_{\min } \leq \mathbf{q}_k+\dot{\mathbf{q}}_k \Delta t+\frac{1}{2} \ddot{\mathbf{q}}_k(\Delta t)^2 \leq \mathbf{q}_{\max },\] \[\dot{\mathbf{q}}_{\min } \leq \dot{\mathbf{q}}_k+\ddot{\mathbf{q}}_k \Delta t \leq \dot{\mathbf{q}}_{\max },\] \[\ddot{\mathbf{q}}_{\min } \leq \ddot{\mathbf{q}}_k \leq \ddot{\mathbf{q}}_{\max } .\]

The robot joints cannot stop in an instant. Applying the maximum deceleration $\ddot{q}_{\min , j}<0$ to the j-th joint for some time $t \geq t_k$ results in the following position and velocity:

\[\left\{\begin{array}{l} q_j(t)=q_{k, j}+\dot{q}_{k, j}\left(t-t_k\right)+\frac{1}{2} \ddot{q}_{\min , j}\left(t-t_k\right)^2 \\ \dot{q}_j(t)=\dot{q}_{k, j}+\ddot{q}_{\min , j}\left(t-t_k\right) \end{array}\right.\]

When the j-th joint reaches its limit $q_{\max , j}$, its velocity should be less or equal to zero. Suppose this event happens at some $t=t^*>t_k$ :

\[\left\{\begin{array}{l} q_j\left(t^*\right)=q_{k, j}+\dot{q}_{k, j}\left(t^*-t_k\right)+\frac{1}{2} \ddot{q}_{\min , j}\left(t^*-t_k\right)^2=q_{\max , j} \\ \dot{q}_j\left(t^*\right)=\dot{q}_{k, j}+\ddot{q}_{\min , j}\left(t^*-t_k\right) \leq 0 . \end{array}\right.\] \[\dot{q}_{k, j} \leq \sqrt{-2 \ddot{q}_{\min , j}\left(q_{\max , j}-q_{k, j}\right)}\] \[\dot{q}_{k, j} \geq-\sqrt{2 \ddot{q}_{\max , j}\left(q_{k, j}-q_{\min , j}\right)}\]

Transformed into a form dependent on $\mathbf{u}_k$.

\[\mathbf{q}_{\min }-\mathbf{A}_0 \mathbf{z}_k \leq \mathbf{B}_0 \mathbf{u}_k \leq \mathbf{q}_{\max }-\mathbf{A}_0 \mathbf{z}_k,\] \[\dot{\mathbf{q}}_{\min }-\mathbf{A}_1 \mathbf{z}_k \leq \mathbf{B}_1 \mathbf{u}_k \leq \dot{\mathbf{q}}_{\max }-\mathbf{A}_1 \mathbf{z}_k,\] \[\ddot{\mathbf{q}}_{\min } \leq \mathbf{u}_k \leq \ddot{\mathbf{q}}_{\max } .\]

The constraints for $\dot{\mathbf{q}}_{k+1}$ can be transformed into:

\[\rho_{\min } \leq \mathbf{u}_k \leq \rho_{\max },\]