To perform complex and informal tasks, modern robots such as dual-arm manipulators, humanoid robots, mobile manipulators have many joints and most of them are kinematically redundant. By exploiting a redundant robotic system, multiple tasks can be executed satisfying various constraints among the infinite number of control solutions.

For practical use, the control solution has to satisfy inequality constraints for joint limits and workspace limit avoidance. In addition, the capability of hierarchical task control is important to manage multiple tasks systematically. Moreover, the aforementioned has to be considered in a fast computation to keep a strict real-time system. For the control with high frequency such as 1 kHz, it has to solve the control problem within 1 ms.

Kinematically redundant manipulators generally solve inverse dynamics or inverse kinematics problems to calculate control solutions. In this paper, we focus on the inverse dynamics problem which requires more computation time compared to the inverse kinematics. In the inverse dynamics, joint space torque that can create desired end-effector motion or force at the Cartesian coordinate can be calculated based on the Jacobian matrix and robot dynamics. Nowadays, inverse dynamics solvers can be classified into three approaches: a method based on pseudo inverse, a method based on quadratic programming (QP), and a method based on hierarchical quadratic programming (HQP).

The operational space formulation (OSF) is a typical and classic method for inverse dynamics control [1]. In the OSF, joint acceleration energy minimized torque solution is calculated through the dynamically consistent inverse, i.e., inertia weighted pseudo inverse. In addition, the OSF can provide strict task priority by removing the effect from lower priority tasks to the higher priority tasks based on the null-space projection approach. However, since the OSF cannot apply inequality constraints explicitly, it requires additional algorithms [2,3] for considering the inequality constraints such as joint torque limits, singularity avoidance, and obstacle avoidance.

Consideration of the inequality constraints are becoming more important for practical use of robots. Thus, the QP-based controller is proposed to consider inequalities when solving inverse dynamics for floating base robots [4]. By solving a numerical quadratic program in the QP-based methods, both equality and inequality constraints can be considered. However, it cannot guarantee a strict task hierarchy although it can set soft task priority. Thus, the lower priority task can affect higher priority tasks.

As an extension of the QP-based method, the HQP-based method is proposed for floating base robots [5]. By solving multiple QPs, the HQP can satisfy equality and inequality constrains and can satisfy a strict hierarchy for task control. However, since it has to solve QPs more than the number of task hierarchies, it requires a large computational cost for multiple task execution.

The HQP-based methods regarding the floating base robots such as humanoids are generally focusing on the computational cost since those robots have high degrees of freedom (DoFs) and many constraints. De Lasa et al. [6] improve the computational time by reducing equality and inequality constraints of lower priority QPs. Herzog et al., Mansard, Lee et al. [7-9] decrease the number of decision variables. Escande et al. [10] suggest a method to reduce the number of QP solving.

Recently, the HQP-based approach is exploited for fixed base robots [11]. Since the number of joints of the fixed base robot is increasing, e.g., dual-arm manipulators [12], a large computational cost is also required. However, methods for improving the computational cost of the HQP for fixed base robots are rare. To the best of our knowledge, a study in [13] is the only method regarding the computational cost issue. Since the dynamics of the floating base robot and the fixed base robot are different, so the aforementioned computational cost reduction approaches for floating base robots can’t be applied directly to the fixed base robot.

Therefore, the main contribution of this study comes from the proposal of the computationally improved HQP formulation for the fixed base robot. Motivated by the approach in the methods for the floating base robots, we reduce the size of decision variables and reformulate the HQP problem. The proposed method has an advantage since it can minimize both the joint space and the task space variables and can consider two or more hierarchical tasks. Compared to our study, the most recent study [13] can consider only two-level tasks and cannot optimize task space variables. Thus, our method can be considered as a more general method.

Real robot experiments are executed on the dual-arm manipulator to evaluate the reduction of computational time compared to the OSF and conventional HQP-based method. In addition, the performance of the proposed method is also evaluated by experiments.

In this section, we introduce the background theory about HQP-based inverse dynamics controller. Rigid body dynamics of the robot with n joints can be expressed as follows:

where M(q) is the inertia matrix, hq˙,q is the sum of the Coriolis/centrifugal force vector, and gravity force vector. And q∈Rn is the joint position vector and τ is the joint torque vector.

The forward kinematics which describes the relationship between task space and joint space, and its differential relationship are shown as

where J(q) is a Jacobian matrix, x∈ Rm is the operational space coordinate vector, and m is the DoFs of task [14]. After this paragraph, Mq, hq˙,q and J(q) are indicated as M, h, and J, respectively for the sake of brevity. Based on equations (1) and (2), the HQP-based controller solves the inverse dynamics solution.

HQP is the process of solving cascade numerical optimization problems using quadratic programming (QP). In the QP, we can minimize quadratic function while satisfying equality and inequality constraints. From the HQP method in [15], the QP formulation for the first priority task can be derived as:

where w₁ is a slack variable vector for relaxing control reference for a feasible solution. Top lines and underlines indicate upper and lower constraints’ bound vectors. x¨d is a desired operational space acceleration vector and the number of subscripts at x¨d, W, J denotes the priority of the tasks. After solving the QP optimization, by applying the optimal torque τ to the robot, the end-effector acceleration can be generated as x¨d1+w1 while satisfying all the constraints. When there is the second priority task, instead of applying τ from above, the second QP is solved with obtained optimal slack variable w₁.

An optimal slack variable w₁ is now expressed as w1* in the second QP and it is set as an additional equality constraint. The second QP can be formulated as follows:

where w₂ is a slack variable for the second priority task. According to the equality constraint regarding w1*, i.e., x¨d1+w1*=J˙1q˙+J1q¨, a strict task hierarchy can be obtained since the second QP will calculate a control solution that does not affect the first priority task. Note that it is proved in [5] that this approach can guarantee strict task hierarchy as the same as the well-known null-space projection-based method [1]. As the result of the second QP, τ generates the end-effector acceleration as x¨d1+w1*, and x¨d2+w2 for the hierarchical tasks.

Similarly, we can consider more hierarchical tasks by solving QPs in cascade manner as introduced above. The generalized equation for a k-th prioritized task can be described as follows:

where the notation of subscript of l indicates a higher priority of tasks than k.

Finally, control torque can be determined as the torque τ from the QP for the last priority task. It is notable that there are still infinite solutions when the hierarchical tasks do not use the full rank of the robot system. Thus, the torque τ may come out with a different solution each time when solving HQP. Since obtaining a continuous torque solution is important for robot control, it is necessary to regularize the solution by solving additional QP as shown below:

In the above QP example, torque is minimized, which can be used to configure various cost functions such as joint kinetic energy or joint acceleration energy. After that, we can finally obtain an optimal solution from the last QP.

In this section, we present a novel QP formulation with reduced decision variables to decrease the computational cost of the HQP-based method. The proposed method can make the computational cost smaller than conventional HQP-based method by resizing the decision variables size while satisfying a dynamical relationship of rigid body. As a result, the proposed method can create the same control solution as the conventional method in Section 2 with a decreased computation time. By reducing the computational cost, it can be more practically used for real-time control with high control frequency.

From (1), joint acceleration and torque relationship can be obtained as follows:

Then, by substituting (10) into (2), i.e., x¨=J˙q˙+Jq¨, we can derive an expression of task space acceleration as

By utilizing (12), joint acceleration term q¨ can be removed still considering both (1) and (2). However, the Coriolis/centrifugal, and gravity force acting on joints cannot be fully compensated by the above formula since h is compensated on the task space by pre-multiplying JM^-1. As mentioned in the previous study [15], compensation of h at the joint space is much efficient than compensating at the task space. Thus, we consider the robot dynamics that excludes the force vector h for solving HQP and include it in the final torque equation after all computations are over. Accordingly, the task space acceleration equation can be described as follows:

The above (13) replaces dynamics and task constraints in the conventional QP formula introduced in the previous section. Thus, the first QP can be reformulated as

Compared to the first QP of the conventional HQP described as (3), the variable q¨ is removed from the decision variable. This is possible since q¨ related constraints are removed by applying (13) as constraint instead of robot dynamics and task-joint relationship equation. Note that the joint acceleration inequality, i.e., q¨_≤q¨≤q¨¯, can be addressed by the joint torque inequality, i.e., τ_≤τ≤τ¯. Therefore, the above can derive the same solution as the conventional method.

Same as the conventional HQP manner introduced in the previous section, lower priority tasks can be solved by the cascade QPs. The QP for a k-th prioritized task can be expressed as follows:

In addition, when the tasks do not require the full rank of the robot, a QP for torque regularization can be solved as (9). It can be expressed as follows:

The resulting torque τ from the last QP is now expressed τ* as an optimal solution for the controlling hierarchical tasks. Finally, the desired torque τ can be determined by adding the Coriolis/centrifugal force h.

Since h is added to τ* in the last QP solution, the compensation for h is completed in joint space as aforementioned. The entire process of proposed method can be seen at [Fig. 1].

[Fig. 1] 
Overview of the algorithm of the proposed method

As can be seen in the above QP formulations, since q¨ is removed, the size of the decision variable reduces from 2n+m_k to n+m_k where m_k is the DoFs of the k-th task. For the QP for regularization, the size of the decision variable reduces from 2n to n. In general, since the task DoFs is smaller than robot DoFs, m_k≤n, our proposed method reduces its size by at least 33%. Accordingly, the total computation time of the HQP reduces. Also, by following the cascade manner of the conventional HQP, the proposed method can achieve the advantages of the previous method.

In this paper, we propose a decision variable reduced HQP-based robot controller. Applying the relationship between task space acceleration with joint space torque leads to a reduction of computational costs. As a result, the reduced HQP can solve an optimization problem much faster than the previous method. Through this, an articulated robot controller that can consider the hierarchical tasks and inequality constraints can be practically used in a system that requires a high control frequency. The performance of the proposed method is verified through real robot experiments.

In our future study, the HQP formulation will be extended to manage workspace and singularity avoidance problems. Since the workspace and singularity are not described explicitly in an equation for the HQP formula, additional study is required for practical use. Accordingly, the practicality of the HQP-based controller can be improved.