Generalized hierarchical control

Abstract

Multi-objective control systems for complex robots usually have to handle multiple prioritized tasks. Most existing hierarchical control techniques handle either strict task priorities by using null-space projectors or a sequence of quadratic programs; or non strict task priorities by using a weighting strategy. This paper proposes a novel approach to handle both strict and non-strict priorities of an arbitrary number of tasks. It can achieve multiple priority rearrangements simultaneously. A generalized projector, which makes it possible to completely project a task into the null-space of a set of tasks, while partially projecting it into the null-space of some other tasks, is developed. This projector can be used to perform priority transitions and task insertion or deletion. The control input is computed by solving one quadratic programming problem, where generalized projectors are adopted to maintain a task hierarchy, and equality or inequality constraints can be implemented. The effectiveness of this approach is demonstrated on a simulated robotic manipulator in a dynamic environment.

Proof of the maintenance of strict hierarchies represented by standard lexicographic orders subject to constraints

This section proves that the proposed GHC approach (13) can maintain strict task hierarchies represented by standard lexicographic orders while accounting for linear constraints.

Suppose there are $n_t$ tasks that should be organized in a way such that each task i has a strict lower priority than task $i-1$ with i = $2,\ldots ,n_t$. In this case, the generalized projector $\mathbf {P}_i$ for a task i is in fact a null-space projector, which projects a task Jacobian into the null-space of all the previous $i-1$ tasks, and each $\mathbf {A}_i$ is an identity matrix. Let each task objective function be $\varvec{f}_{i} = \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d$, with $\varvec{x}_i^{\prime }$ being a joint space task variable, such as $\dot{\varvec{q}}_{i}^{{\prime }}$, $\ddot{\varvec{q}}_{i}^{{\prime }}$, or $\varvec{\tau }_{i}^{{\prime }}$, etc. Moreover, the global variable $\varvec{x}=\sum _i \mathbf {P}_i\varvec{x}_i^{\prime }$ should satisfy linear equality or inequality constraints $\mathbf {G} \varvec{x}\le \varvec{h}$.

At the first stage, the regulation term is neglected, and the optimization problem can be written as follows

$$\begin{aligned} \begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(n_t)}^{\prime } \end{array}}&\sum \limits _{i=1}^{n_{t}} \left\| \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d \right\| ^2 \\ \text {subject to}\;&\mathbf {G}\sum \limits _{i=1}^{n_{t}} \mathbf {P}_i\varvec{x}_i^{\prime } \le \varvec{h} \end{aligned} \end{aligned}$$

(16)

where ${\varvec{x}}_{(n_t)}^{\prime }=\left\{ \varvec{x}_{1}^{\prime },\varvec{x}_{2}^{\prime },\ldots \varvec{x}_{n_t}^{\prime }\right\}$, and the solution to (16) is denoted as $\varvec{x}_{(n_t)}^*=\left\{ \varvec{x}_{1}^{*},\varvec{x}_{2}^{*},\ldots \varvec{x}_{n_t}^{*}\right\}$.

When $n_t = 1$, the optimization problem can be written as

$$\begin{aligned} \begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(1)}^{\prime } \end{array}}&\left\| \mathbf {J}_1 \varvec{x}_{(1)}^{\prime } - \varvec{x}_1^d \right\| ^2 \\ \text {subject to}\;&\mathbf {G} \varvec{x}_{(1)}^{\prime } \le \varvec{h}. \end{aligned} \end{aligned}$$

(17)

The solution to this problem $\varvec{x}_{(1)}^*$ is the same as the one to the problem formulated by HQP.

When $n_t = k$, the optimization problem is formulated as

$$\begin{aligned} \begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(k)}^{\prime } \end{array}}&\sum \limits _{i=1}^{k} \left\| \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d \right\| ^2\\ \text {subject to}\;&\mathbf {G}\sum \limits _{i=1}^{k} \mathbf {P}_i\varvec{x}_i^{\prime } \le \varvec{h}. \end{aligned} \end{aligned}$$

(18)

Suppose the solution $\varvec{x}_{(k)}^*$ can maintain the strict task hierarchy: if a task $k+1$ is inserted with lowest priority with respect to the set of k tasks, then the optimization problem with the $k+1$ tasks can be written as

$$\begin{aligned} \begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(k+1)}^{\prime } \end{array}}&\sum \limits _{i=1}^{k} \left\| \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d \right\| ^2 + \left\| \mathbf {J}_{k+1} \varvec{x}_{k+1}^{\prime } - \varvec{x}_{k+1}^d \right\| ^2 \\ \text {subject to}\;&\mathbf {G}\left( \sum \limits _{i=1}^{k} \mathbf {P}_i\varvec{x}_i^{\prime } + \mathbf {P}_{k+1}\varvec{x}_{k+1}^{\prime }\right) \le \varvec{h}. \end{aligned} \end{aligned}$$

(19)

As $\mathbf {P}_{k}\mathbf {P}_{k+1}=\mathbf {P}_{k+1}$, the term $\sum \limits _{i=1}^{k} \mathbf {P}_i\varvec{x}_i^{\prime } + \mathbf {P}_{k+1}\varvec{x}_{k+1}^{\prime }$ in the constraint in (19) is equivalent to $\sum \limits _{i=1}^{k-1} \mathbf {P}_i\varvec{x}_i^{\prime } + \mathbf {P}_{k}\varsigma _k$, with

$$\begin{aligned} \varsigma _k = \varvec{x}_k^{\prime } + \mathbf {P}_{k+1}\varvec{x}_{k+1}^{\prime }. \end{aligned}$$

(20)

Then problem (19) can be written as

$$\begin{aligned} \begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(k)}^{\prime },\varsigma _k,\varvec{x}_{k+1} \end{array}}&\sum \limits _{i=1}^{k-1} \left\| \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d \right\| ^2 + \left\| \mathbf {J}_k \varsigma _k - \varvec{x}_k^d \right\| ^2 \\&+\left\| \mathbf {J}_{k+1} \varvec{x}_{k+1}^{\prime } - \varvec{x}_{k+1}^d \right\| ^2 \\ \text {subject to}\;&\mathbf {G}\left( \sum \limits _{i=1}^{k-1} \mathbf {P}_i\varvec{x}_i^{\prime }+\mathbf {P}_k\varsigma _k\right) \le \varvec{h}\\&\varsigma _k = \varvec{x}_k^{\prime } + \mathbf {P}_{k+1}\varvec{x}_{k+1}^{\prime }. \end{aligned} \end{aligned}$$

(21)

$\varvec{x}_k^{\prime }$ in (21) is a free variable, and this problem can be separated into two sub-problems. The first sub-problem is

$$\begin{aligned} \begin{aligned}&\mathop {{\arg \min }}\limits _{\begin{array}{c} {\varvec{x}}_{(k-1)}^{\prime },\varsigma _k \end{array}} \sum \limits _{i=1}^{k-1} \left\| \mathbf {J}_i \varvec{x}_i^{\prime } - \varvec{x}_i^d \right\| ^2 + \left\| \mathbf {J}_k \varsigma _k - \varvec{x}_k^d \right\| ^2\\&\text {subject to}\;\ \mathbf {G}\left( \sum \limits _{i=1}^{k-1} \mathbf {P}_i\varvec{x}_i^{\prime }+\mathbf {P}_k\varsigma _k\right) \le \varvec{h}. \end{aligned} \end{aligned}$$

(22)

The optimal solution $\sum \limits _{i=1}^{k-1}\mathbf {P}_i\varvec{x}_{i}^{*,\prime } + \mathbf {P}_k\varsigma _k^*$ to this problem is equivalent to the one of (18). Indeed, these two solutions have the same effect on task k

$$\begin{aligned} \mathbf {J}_k\sum \limits _{i=1}^{k}\mathbf {P}_i\varvec{x}_{i}^{*,\prime } = \mathbf {J}_k\left( \sum \limits _{i=1}^{k-1}\mathbf {P}_i\varvec{x}_{i}^{*,\prime } + \mathbf {P}_k\varsigma _k^*\right) . \end{aligned}$$

(23)

To prove (23), one needs to notice that $\mathbf {J}_i\mathbf {P}_{j}=\varvec{0}$ with $j\ge i$. The second sub-problem is given by

$$\begin{aligned} \mathop {{\arg \min }}\limits _{\begin{array}{c} \varvec{x}_{k+1} \end{array}} \left\| \mathbf {J}_{k+1} \varvec{x}_{k+1}^{\prime } - \varvec{x}_{k+1}^d \right\| ^2. \end{aligned}$$

(24)

Therefore, the insertion of a lower priority task $k+1$ does not change the optima of the k previous task objectives. In other words, the strict task hierarchy of an arbitrary number of tasks subject to linear constraints can be maintained.

We have proved that each lower priority task will not increase the obtained optima of all the previous tasks. The rest of this proof explains the roles of the regulation term. As mentioned in Sect. 5, the use of a regulation term, which minimizes the norm of each task variable, helps to ensure the uniqueness of the solution. As each task objective i is assigned with the weight $\omega _i=1$, which is much greater than the weight of the regulation term ($\omega _r<<1$), the task variables are optimized to mainly satisfy task objectives. Moreover, in GHC, this regulation term also helps to improve the performance of lower priority tasks. Consider $k+1$ levels of tasks to handle, as $\mathbf {J}_i\mathbf {P}_{j}=\varvec{0}$ with $j\ge i$, the final solution is $\sum \limits _{i=1}^{k} \mathbf {P}_i\varvec{x}_{i}^{*} + \mathbf {P}_{k+1}\varvec{x}_{k+1}^{*}$. Denoting the elements required by task i as $\varvec{x}_{i}^{i,*}$ and the rest elements that are are not effectively handled by task objective i as $\varvec{x}_{i}^{f,*}$, the final solution can be rewritten as $S=\sum \limits _{i=1}^{k} \mathbf {P}_i^i\varvec{x}_{i}^{i,*} + \sum \limits _{i=1}^{k} \mathbf {P}_i^f\varvec{x}_{i}^{f,*} +\mathbf {P}_{k+1}\varvec{x}_{k+1}^{*}$, with $\mathbf {P}_i^i$ and $\mathbf {P}_i^f$ the columns in $\mathbf {P}_i$ that correspond to $\varvec{x}_{i}^{i,*}$ and $\varvec{x}_{i}^{f,*}$ respectively. The term $\sum \limits _{i=1}^{k} \mathbf {P}_i^f\varvec{x}_{i}^{f,*}$ that is not required by the k previous tasks may contribute to task $k+1$ and affect its task performance. The minimization of the norm of $\varvec{x}_{i}^{f}$ in the regulation term improves the performance of task $k+1$ by making S closer to $\sum \limits _{i=1}^{k} \mathbf {P}_i^i\varvec{x}_{i}^{i,*}+\mathbf {P}_{k+1}\varvec{x}_{k+1}^{*}$, where $\mathbf {P}_i^i\varvec{x}_{i}^{i,*}$ are used to perform the k previous tasks and $\mathbf {P}_{k+1}\varvec{x}_{k+1}^{*}$ is used to perform the $(k+1)\mathrm{{th}}$ task in the null-space of all the higher priority tasks.