Inverse Kinematic Control of Humanoids under Joint Constraints

Abstract

We propose an inverse kinematic control framework for a position controlled humanoid robot with bounded joint range, velocity, and acceleration limits. The proposed framework comprises two components, an inverse kinematics algorithm and a damping controller. The proposed IKTC (Inverse Kinematics with Task Corrections) algorithm is based on the second order task-priority method in order to ensure the velocity-continuity of the solution. When the minimum norm solution exceeds the joint bounds, the problem is treated as a quadratic optimization problem with box constraints; an optimal task correction that lets the solution satisfy the constraints is found. In order to express the three kinds of joint constraints as a second order box constraint, a novel method is also proposed. The joint stiffness of a position controlled humanoid robot necessitates a damping controller to attenuate jolts caused by repeated contacts. We design a damping controller by using an inverted pendulum model with a compliant joint that takes into account the compliance around the foot. By using ZMP [20] measurement, the proposed damping controller is applicable not only in SSP (Single Support Phase) but also in DSP (Double Support Phase). The validity of the proposed methods is shown by imitating a captured whole-body human motion with a position controlled humanoid robot.

Keywords

Inverse Kinematics Joint Constraint Damping Controller Humanoid Robot

1. Introduction

The ultimate purpose of humanoid robots is to provide convenience for humans in human living environments. In order for humanoids to assist or substitute for humans, reliable mobility in various environments and dexterous manipulability are required. Therefore, humanoids are designed to have structural similarities to humans, i.e., two arms and two legs. With this structural characteristic a humanoid robot is able to perform multiple tasks simultaneously. However, this design introduces many degrees of freedom (DOF) and makes dynamic problems very complicated. A position-controlled humanoid, in this sense, allows simpler kinematic approaches.

The task-priority method [13, 18], which uses task Jacobian and its null space projection operators, is very suitable to handle the kinematic problems of a humanoid robot in multiple tasks. But simple inversion of the Jacobian does not take into account unilateral constraints such as the joint range, velocity, and acceleration limits. For this reason, several methods have introduced tasks to cope with unilateral constraints. The potential field is a task that pushes joints away from their limit positions [9]. An objective function that has a minimum value at the centre of the joint range is used in the gradient projection method [11]. However, the priorities of these tasks are always lower than those of the primary tasks, and thus satisfaction of the constraint is not guaranteed. In addition, it is difficult to select appropriate parameters that determine the strength of the repulsion or attraction. A method using the weighted least-norm solution was proposed in [3] to overcome the shortcomings of the gradient projection method; however, joint velocity and acceleration limits were not taken into account.

Clamping the joint velocity and acceleration to the limit values is the simplest way to satisfy the constraints but may result in unpredictable task errors. By using the infinity-norm, the lowest possible joint velocity solution was obtained in [5]. However, it is not guaranteed that the lowest velocity solution will satisfy the velocity limit. In [2, 1], the task-space trajectory was slowed down when the joint velocity or acceleration limits were encountered.

A typical joint position controller uses a high-gain PD control and thus discontinuous position commands should be avoided to protect joints from being damaged. For this reason, not only joint range but also joint velocity and acceleration limits should be considered simultaneously. A unified framework in which the three kinds of joint constraints are explicitly taken into account was proposed in [6]. The method has the same starting point as ours in shaping the joint velocity bounds. However, our inverse kinematics algorithm is defined at the acceleration level and is different from their method since they dealt with a redundant manipulator, whereas we are interested in a humanoid robot, which may not be redundant due to multiple tasks. From the aspect of multiple tasks, a general framework was proposed in [8] for a system with prioritized equality and/or inequality tasks. However, the constraints considered in this paper always have the highest priority and this allows a more efficient algorithm to be developed.

The high-gain position controllers of a humanoid robot introduce stiffness of joints and make the system vulnerable to impact forces. Even a small impact on the foot may cause a jolt to the system since a humanoid robot is not fixed on the ground. A damping controller can be used to reduce the jolt but normally high control bandwidth is required. For this reason, compliant materials are used, as shown in Figure 9, to absorb impacts on the contact surface; these materials function as a low-pass filter, lowering the required control bandwidth. Thus the damping controller should take into account the effect of these compliant materials.

Figure 1.

The proposed inverse kinematic control scheme.

Figure 2.

A cart-table model for the proof of Proposition 3.1.

Figure 3.

3R planar manipulator for the simulation in 3.3.

Figure 4.

Tracking of the task trajectory (red circles) under joint limit constraints using (a) GF and (b) IKTC.

Figure 5.

Trajectories of q₁, q̇₁, and q̈₁ corresponding to the simulation results in Figure 4.

Figure 6.

Trajectories of q₂, q̇₂, and q̈₁ corresponding to the simulation results in Figure 4.

Figure 7.

Trajectories of q₃, q̇₃, and q̈₃ corresponding to the simulation results in Figure 4.

Figure 8.

Error trajectories of the three tasks in the simulation in Section 3.3.

Figure 9.

Compliant materials around the foot of humanoid robot Hubo.

In this paper, we propose an inverse kinematic control framework for a position controlled humanoid robot that has constraints of joint range, velocity, and acceleration limits. The proposed framework is comprised of two components, an inverse kinematics algorithm and a damping controller, as shown in Figure 1.

The proposed inverse kinematics algorithm, IKTC, is based on the second order task-priority method. When the minimum norm solution exceeds the joint bounds, an optimal task correction that would let the solution reside within the bounds is found by using quadratic programming. While the general method proposed in [8] has to solve a cascade of quadratic programs, IKTC solves a quadratic program once and thus can increase computational efficiency. In order to express the three kinds of constraints as a second order box constraint, a novel method is also proposed.

The proposed damping controller uses an inverted pendulum model with a compliant joint (IPCJ), as in [10]. While the damping controller proposed in [10] had the purpose of attenuation of the vibration in SSP (single support phase) only, our damping controller uses ZMP measurement and is applicable not only in SSP but also in DSP (double support phase).

The proposed methods are validated by imitating a captured human motion with a position controlled humanoid robot. Since there are kinematic and dynamic differences between humans and humanoids, the imitation of a captured human motion will be a good challenge to verify the proposed methods.

This paper is organized as follows. In Section 2, the background of the proposed methods is given. Section 3 presents the inverse kinematics algorithm and Section 4 presents the damping controller. The experiments are presented in Section 5 and conclusions are drawn in Section 6.

2. Background

2.1 Task-Priority based Inverse Kinematics

As mentioned earlier, a humanoid robot has a lot of DOF and thus is capable of performing multiple tasks simultaneously. However, a solution does not exist that is able to achieve all the tasks when there is a conflict. The least-squares solution can be used to minimize the second-norm of task errors when there is no exact solution, though this may not satisfy any of the tasks exactly.

The task-priority strategy is devised to handle tasks according to their order of priority [7,12,13]. By executing a task of lower priority in the null space of a higher priority task, conflicts between tasks are dealt with. A general framework of the methodology is proposed in [18] and is briefly reviewed in the following.

Consider a kinematic equation for the i-th task

x_{i} = f_{i} (q)

(1)

where $x_{i} \in ℝ^{m_{i}}$ is the i-th task vector, $q \in ℝ^{n}$ is the joint position vector, and i is the order of priority. The second-order differential kinematic equation for the i-th task can then be expressed as

J_{i} \dot{q} = h_{i}

(2)

where $J_{i} = \partial f_{i} ∕ \partial q \in ℝ^{m_{i} \times n}$ is the Jacobian matrix of the i-th task and h_i is defined as

h_{i} ≜ {\dot{x}}_{i} - {\dot{J}}_{i} \dot{q} .

(3)

The null space projection operator of the i-th task N_i is given by

N_{i} = I - J_{i}^{+} J_{i}

(4)

where the superscript + denotes the pseudo-inverse of a matrix and I is an n × n identity matrix.

If we let the (i−1)th task have priority over the i-th task, and assume that the sum of all tasks' dimensions does not exceed the system's DOF, i.e., $\sum_{i} m_{i} \leq n$ , then the joint

acceleration that achieves higher i tasks (from the first to the i-th task) is given by

{\dot{q}}_{i} = {\dot{q}}_{i - 1} + {\bar{J}}_{i}^{+} (h_{i} - J_{i} {\dot{q}}_{i - 1}), ({\dot{q}}_{1} = J_{1}^{+} h_{1})

(5)

with

{\bar{J}}_{i} = J_{i} N_{i - 1}^{A},

(6)

N_{i}^{A} = N_{i - 1}^{A} - {\bar{J}}_{i}^{+} {\bar{J}}_{i}, (N_{0}^{A} = I) .

(7)

As shown in (5), q̈_i can be divided into two components, the solution of previous (i−1) tasks, q̈_i-1, and the solution that achieves the i-th task in the null space of the previous tasks by using the inverse of the task-consistent Jacobian J̄_i given in (6). The null space projection operator given in (7) projects any joint space vector onto the null space of

previous tasks and therefore any components that would disturb the previous tasks are excluded by the task-consistent Jacobian.

2.2 Damped Least-Squares Inverse

When a task Jacobian loses its rank, the task cannot be executed and it is called a kinematic singularity. Another singularity called an algorithmic singularity occurs when a task-consistent Jacobian J̄_i is rank-deficient. In other words, the i-th task is incompatible with the higher (i−1) tasks. With these singularities, the inverse of J̄_i becomes ill-conditioned and the solution (5) tends to infinity.

To handle the singularities, we use the damped least-squares method proposed in [19]. The damped least-squares (DLS) inverse is given by

J^{λ +} = {(J^{T} J + λ^{2} I)}^{- 1} J^{T}

(8)

or, equivalently,

J^{λ +} = J^{T} {(J J^{T} + λ^{2} I)}^{- 1}

(9)

where λ ∈ ℝ is the damping factor.

The DLS inverse (8) provides the solution of

\min_{\dot{q}} \frac{1}{2} {‖ J \dot{q} - h ‖}^{2} + λ^{2} {‖ \dot{q} ‖}^{2} .

(10)

Therefore, the solution norm is bounded to a certain value and singularity-robustness is obtained. However, the DLS inverse is not so sophisticated that the solution satisfies the given joint constraints. Thus we use the DLS inverse only for regularization of the solution in the neighbour of the singularity.

2.3 Closed-Loop Inverse Kinematics (CLIK)

In the task-priority strategy, lower priority tasks are sacrificed to achieve higher priority tasks when there is a conflict between the tasks. The error due to the sacrifice cannot be compensated for by the open-loop solution (5) even when there is no conflict afterwards. Besides, numerical integration of the differential inverse kinematic solution (5) would introduce long-term drift of the reconstructed profile from the desired profile. In order to deal with this problem, we adopt a second-order closed-loop inverse kinematics (CLIK) scheme [17].

For the generic i-th task, the CLIK scheme can be implemented as

{\dot{x}}_{i} = {\dot{x}}_{i, d} + k_{v} ({\dot{x}}_{i, d} - {\dot{x}}_{i}) + k_{p} (x_{i, d} - x_{i})

(11)

where the subscript d denotes the desired value; k_v and k_p are the control gains for velocity and position error, respectively. If we let ei denote the error between the desired value and the actual value of the i-th task, (11) can be rewritten as

{\dot{e}}_{i} + k_{v} {\dot{e}}_{i} + k_{p} e_{i} = 0

(12)

where e_i = x_i,d - x_i. Eq. (12) is the error dynamics of the i-th task; the convergence of the error can be shaped by selecting appropriate k_v and k_p.

2.4 Consideration of Linear Inequality Constraints Using Quadratic Programming

In the presence of joint limits, an inverse kinematics problem can be treated as a quadratic optimization problem with linear inequality constraints such as

\begin{matrix} \min \frac{1}{2} {‖ J \dot{q} - h ‖}^{2} \\ subject to b_{l} \leq \dot{q} \leq b_{u} \end{matrix}

(13)

where b_u ∈ ℝⁿ and b_l ∈ ℝⁿ are the upper and lower bounds of the joint acceleration, respectively. The optimal solution of (13) can be found by using a typical quadratic programming (QP) algorithm [15].

The optimization algorithm proposed in [8] uses a sequence of QP in order to handle prioritized tasks. From the highest to the lowest priority task, QP is carried out during each task to solve an optimization problem given in the form of (13); the solution of the current priority task is imposed as a linear equality constraint on the next priority task. This algorithm is generous such that not only prioritized linear equality tasks but also prioritized linear inequality tasks can be taken into account. However, since here the constraints of the joint range, velocity, and acceleration limits always have the highest priority, a more efficient algorithm could possibly be developed.

3. Inverse Kinematics Algorithm

3.1 Joint Range, Velocity, and Acceleration Limits

Since we deal with inverse kinematics problems at the acceleration level, the joint range, velocity, and acceleration limits should be taken into account in the form of a second-order inequality constraint, as shown in (13).

Proposition 3.1

If the joint acceleration q̈ satisfies the inequality (14), the joint position, velocity, and acceleration are kept below their limits, respectively,

b_{l} \leq \dot{q} \leq b_{u}

(14)

\begin{matrix} {\begin{cases} b_{u} = \min (a_{\max}, \frac{v_{\max} - \dot{q}}{Δ t}, \frac{\sqrt{2 a_{\max} (q_{\max} - q)} - \dot{q}}{Δ t}) \\ b_{l} = \max (- a_{\max}, \frac{- v_{\max} - \dot{q}}{Δ t}, \frac{- \sqrt{2 a_{\max} (q - q_{\min})} - \dot{q}}{Δ t}) \end{cases} \end{matrix}

where q_min, q_max, v_max, a_max are given values for the minimum and maximum joint position limits and the maximum velocity and acceleration limits, respectively.

Proof) Since the first and second terms in the min/max function are trivial constraints for the joint acceleration and velocity limits, respectively, the proof is omitted.

By the third term in the min/max function, the joint velocity q is constrained such that

- \sqrt{2 a_{\max} (q - q_{\min})} \leq \dot{q} \leq \sqrt{2 a_{\max} (q_{\max} - q)},

(15)

and, as the result of this, the joint position is kept inside the position range [q_min,q_max]. As for the proof, we show that the joint position gets out of the range if (15) does not hold.

Consider a cart of unit mass on a table, as shown in Figure 2. Let us assume that the initial velocity of the cart is

{\dot{q}}_{0} = k \sqrt{2 a_{\max} (q_{\max} - q_{0})}, (k > 1 : const .),

(16)

and that it decelerates maximally so as not to fall from the table. From the viewpoint of energy, the kinetic energy of the cart should be zero, at least on the edge of the table, in order not to fall. By applying the principle of the conservation of the energy, the cart has kinetic energy KE_@qmax at q = q_maxand KE_@qmax is given by

\begin{matrix} K E_{@ q \max} = K E_{@ q_{0}} + W_{d e c} \\ = \frac{1}{2} k^{2} 2 a_{\max} (q_{\max} - q_{0}) - a_{\max} (q_{\max} - q_{0}) \\ = (k^{2} - 1) a_{\max} (q_{\max} - q_{0}) > 0 (∵ k > 1) \end{matrix}

(17)

where W_dec is the work done by an external force during deceleration. Since k was assumed to be k>1, the kinetic energy KE_@qmax is greater than zero and therefore the cart falls from the table. In order for the cart not to fall from the table, k must be |k| < 1 and the right inequality in (15) has been proved. The left inequality in (15) can be proved in the same way.

3.2 Inverse Kinematics with Task Correction (IKTC) Algorithm

In our inverse kinematics algorithm, some or all tasks are corrected such that the joint acceleration solution satisfies the inequality constraints (14) and the corrections are determined by solving a quadratic optimization problem.

Before proceeding, it should be mentioned that an over-constrained system is not considered in this paper. We are interested in a system that has DOF more than or equal to the task dimension. Thus,

n \geq \sum_{i = 1}^{n_{T}} m_{i}

(18)

where n is the system DOF, mi is the dimension of the i-th task, and n_T is the number of tasks.

If we let the correction of the i-th task be Δh_i ∈ℝ^mi, then, by using (5), the joint acceleration solution q̈*_i that satisfies the inequality constraints is obtained by

\begin{array}{l} {\dot{q}}_{1}^{*} = J_{1}^{+} (h_{1} + Δ h_{1}) \\ {\dot{q}}_{2}^{*} = {\dot{q}}_{2}^{*} + {\bar{J}}_{2}^{+} (h_{2} + Δ h_{1} - J_{1} {\dot{q}}_{1}^{*}) \\ ⋮ \\ {\dot{q}}_{i}^{*} = {\dot{q}}_{i - 1}^{*} + {\bar{J}}_{i}^{+} (h_{i} + Δ h_{i} - J_{i - 1} {\dot{q}}_{i - 1}) \end{array} .

(19)

For given n_T tasks, (19) can be rewritten as

\begin{array}{l} {\dot{q}}_{1}^{*} = {\dot{q}}_{1}^{​} + J_{1}^{+} Δ h_{1} \\ {\dot{q}}_{2}^{*} = {\dot{q}}_{2}^{​} + J_{1}^{+} Δ h_{1} + {\bar{J}}_{2}^{+} (Δ h_{2} - J_{2} J_{1}^{+} Δ h_{1}) \\ ⋮ \\ {\dot{q}}_{n_{T}}^{*} = {\dot{q}}_{n_{T}}^{​} + G {[\begin{matrix} Δ h_{1}^{T} & Δ h_{2}^{T} & \dots & Δ h_{n_{T}}^{T} \end{matrix}]}^{T} \end{array}

(20)

with

\begin{matrix} G = [\begin{matrix} g_{1} & g_{2} & \dots & g_{n_{T}} \end{matrix}] \\ (g_{i} : n \times m_{i} matrix for i = 1, \dots, n_{T}) \end{matrix}

(21)

where q̈₁,…q̈_nT are joint accelerations obtained by (5) and g₁,…,g_nT are determined by collecting the coefficient matrices T of Δh_i's. Therefore, with the proper manipulation of Δh_i's, G can be obtained easily using (5); this process is described in Algorithm 1.

Remark 1. If there is neither an algorithmic singularity nor a kinematic singularity, then G has full column rank

since the dimension of G is $n \times \sum_{i = 1}^{n_{T}} m_{i}$ and $n \geq \sum_{i = 1}^{n_{T}} m_{i}$ .

Furthermore, using DLS inverse J̄^λ+ instead of J̄⁺ in Algorithm 1, it can be guaranteed that G always has full rank even when there are singularities.

As shown in (20), q̈*_nT is composed of two components, the joint acceleration q̈*_nT from original tasks and the corrective task term by which the inequality constraints are satisfied. Substituting q̈*_nT into the inequality constraints (14) gives

b_{l} - {\dot{q}}_{n_{T}}^{​} \leq G {[\begin{matrix} Δ h_{1}^{T} & Δ h_{2}^{T} & \dots & Δ h_{n_{T}}^{T} \end{matrix}]}^{T} \leq b_{u} - {\dot{q}}_{n_{T}}^{​},

(22)

Algorithm 1:

Obtaining G in (20)

1: for i = 1 to n_T do 2: γ←[0 …0]^T∈ℝ^mi 3: for j = 1 to m_i do 4: Substitute i 1 for the j-th element of γ and 0 for the other elements. 5: Ψ→J̄_i⁺γ 6: for k = i + 1 to n_T do 7: Ψ←(I_n-J̄_k+J_k)Ψ 8: end for 9: Substitute Ψ for the j-th column vector of g_i. 10: end for 11: Substitute gi for the i-th submatrix of G in (21) . 12: end for

Any task corrections satisfying (22) can be chosen as the solution for q̈*_nT. However, since the task corrections are equal to the task errors, and there are priorities given to tasks, the task corrections should be as small as possible and should avoid higher priority tasks as much as possible. For these purposes, let us consider an optimization problem:

\begin{matrix} \min \frac{1}{2} {‖ Δ h_{i}^{a} ‖}^{2} \\ subject to b_{l} - {\dot{q}}_{n_{T}}^{​} \leq g_{i}^{a} Δ h_{i}^{a} \leq b_{u} - {\dot{q}}_{n_{T}}^{​} \end{matrix}

(23)

where Δh^a_i and g^a_i are defined by

Δ h_{i}^{a} ≜ [\begin{matrix} Δ h_{i} \\ ⋮ \\ Δ h_{n_{T}} \end{matrix}], g_{i}^{a} ≜ [\begin{matrix} g_{i} & \dots & g_{n_{T}} \end{matrix}] .

(24)

Solving (23) begins with Δh^a_nT and g^a_nT in order to avoid higher priority task errors. However, a solution may not exist because g^a_nT has rank of m_nT and m_nT ≤n. The existence of the solution is checked using the simplex method [15]; if there does not exist a no solution exists satisfying the inequality constraints, (23) is extended to higher priority tasks by taking Δh^a_i-1 andg^a_i-1. These steps are repeated until a solution is found: this process is summarized in Algorithm 2. If we assume that there is a solution when i = k, the solution Δh^a_k of (23) is then substituted into q̈*_nT with g^a_k and q̈_nT becomes

{\dot{q}}_{n_{T}}^{*} = {\dot{q}}_{n_{T}}^{​} + g_{k}^{a} Δ h_{k}^{a} .

(25)

Algorithm 2:

Solving a quadratic optimization problem (23) taking into consideration the task priority.

1: for i = n_T to 1 do 2: Check if there exists a solution of (23) by using the simplex method. 3: if there is a solution then 4: Solve (23) using QP to obtain Δh ^a_i. 5: break 6: end if 7: end for

The joint acceleration solution q̈*_nT given in (25) satisfies the inequality constraints by virtue of the correction tasks Δh_k,…,Δh_nT. ‘n_T

Before the solution has been obtained, the costly QP is carried out once, which increases the efficiency of the proposed algorithm remarkably better than does the use of the general method proposed in [8].

3.3 Simulation

A 3R planar manipulator, shown in Figure 3, is considered here to present the simulation. All links have the same unit length and the inverse kinematics is solved for the three prioritized tasks. Each joint has position range, velocity, and acceleration limits and, due to these constraints, task corrections happen in the IKTC algorithm. The simulation was conducted using two methods, the general framework (GF) proposed in [8] and the IKTC algorithm. In order to consider the joint constraints in GF, we used the inequality constraints (14) in GF.

The joint range limits of the manipulator are q_max = (180°, 120°, 120°), q_min = (60°,-120°,-120°). The joint velocity and acceleration limits are v_max = 180°/s, a_max = 1800°/s² for all joints. The initial configuration is q(t₀) = (90°,-90°,90°) and thus P_y(t₀) = 2, P_xt₀ = 1, θ₃(t₀) = 90°. The desired P trajectory is a smooth sinusoidal curve connecting 1 and 2 for one second.

Figure 4 shows the simulation results generated by the GF and IKTC algorithms. Both sets of results show that the error of the lowest priority task increases when the three tasks become infeasible as the tip moves right and q₁ reaches its range limit. But, owing to the error, higher priority tasks can be achieved without violating the joint constraints. This can be verified in Figure 8, in which the task errors are shown.

The position, velocity, and acceleration trajectories of the three joints are shown in Figures 5, 6, and 7. None of these have violated their limits and thus the proposed inequality constraint in (14) is validated.

From Figure 8, it can be said that both the GF and IKTC algorithms generated almost the same results. In this simulation, however, GF carried out QP three times at every time step while IKTC carried out QP once at most. This leads to a situation in which the computation cost of IKTC is less than that of the GF algorithm.

4. Damping Controller

4.1 Inverted Pendulum Model with a Compliant Joint (IPCJ)

The compliance around the foot of a humanoid robot can be thought of as a combination of translational springs and rotational springs, as illustrated in Figure 10. Since the translational movement is negligible and a position controlled humanoid robot can be regarded as a rigid body in a moment, we use the IPCJs as illustrated in Figure 11 for the damping controller. We use an IPCJ even in DSP because the coupling moment due to translational springs is not ignorable in DSP, as shown in Figure 10.

Figure 10.

Inverted pendulum models with springs that represent compliances around feet. (a) SSP (b) DSP.

Figure 11.

IPCJs defined in local y˜z˜ planes in (a) DSP and (b) SSP.

In each of the supporting phases, we define a local coordinate frame x˜y˜z˜, as shown in Figure 11, and apply damping controllers in the x˜z˜ plane and y˜z˜ plane, respectively. Figure 11 depicts only the two IPCJs used in the y˜z˜ plane, but two more IPCJs are used in the x˜y˜ planes in the same way.

The linearized equation of motion (EOM) of an IPCJ is

m l^{2} \dot{θ} + b \dot{θ} - m g l θ = τ = - K (θ - u)

(26)

where m and l are the mass and length of the pendulum, θ denotes the actual inclined angle due to the compliance, and u is the reference angle derived from the reference position of the centre of mass (COM) of the humanoid robot. K is the stiffness and b is the damping coefficient of the compliance; τ is the exerted torque on the pendulum and g is gravity.

In designing a damping controller, we use ZMP rather than the torque τ because ZMP can be measured not only in SSP but also in DSP.

If we choose ZMP as the output of IPCJ, then the state-space representation of IPCJ is

\frac{d}{d t} [\begin{matrix} θ \\ \dot{θ} \end{matrix}] = \underset{A}{\underset{⏟}{[\begin{matrix} 0 & 1 \\ \frac{l}{g} - \frac{K}{m l^{2}} & \frac{b}{m l^{2}} \end{matrix}]}} [\begin{matrix} θ \\ \dot{θ} \end{matrix}] + \underset{B}{\underset{⏟}{[\begin{matrix} 0 \\ \frac{K}{m l^{2}} \end{matrix}]}} u,

(27)

y_{Z M P} = \underset{C}{\underset{⏟}{[\begin{matrix} \frac{K}{m g} & \frac{b}{m g} \end{matrix}]}} [\begin{matrix} θ \\ \dot{θ} \end{matrix}] \underset{D}{\underset{⏟}{- \frac{K}{m g}}} u .

(28)

The measured ZMP is given by

[\begin{matrix} {\tilde{x}}_{Z M P} \\ {\tilde{y}}_{Z M P} \end{matrix}] = \frac{1}{f_{R F z} + f_{L F z}} [\begin{matrix} - τ_{\tilde{y}} \\ τ_{\tilde{x}} \end{matrix}],

(29)

[\begin{matrix} τ_{\tilde{x}} \\ τ_{\tilde{y}} \\ τ_{\tilde{z}} \end{matrix}] = {\vec{τ}}_{L F} + {\vec{τ}}_{R F} + {\vec{O P}}_{R F} \times {\vec{f}}_{R F z} + {\vec{O P}}_{L F} \times {\vec{f}}_{L F z}

(30)

where f_RFz and f_LFz are measured normal forces, and ${\vec{τ}}_{R F}$ and ${\vec{τ}}_{L F}$ are measured torques on the right foot and the left foot, respectively. ${\vec{O P}}_{R F}$ and ${\vec{O P}}_{L F}$ denote the position vectors of the right foot and the left foot in the x˜y˜z˜ coordinate frame.

4.2 Model Identification

The parameters of IPCJ can be identified through frequency response methods. We measured sinusoidal responses for various input frequencies with our humanoid robot Hubo [16] in four cases: x˜z˜-DSP, y˜z˜-DSP, x˜z˜-SSP, and y˜z˜-SSP; one set of results is presented in Figure 12.

Figure 12.

Bode plot of $\frac{{\tilde{y}}_{Z M P}}{u}$ in the local y˜z˜ plane in DSP.

In Figure 12, the measured response shows increasing phase shift and nonzero slope of the magnitude plot in the high frequency domain. This means time delay and unmodelled dynamics exist in addition to IPCJ. Since the slope of the magnitude plot in the high frequency domain is about −20dB/dec, the unmodelled dynamics can be approximated by a first-order model. With the Pade approximation [14] of the time delay and the additional use of the first-order model, the transfer function of IPCJ is given by

\frac{y_{Z M P}}{u} = (\frac{- \frac{K}{m g} (s^{2} + \frac{g}{l})}{s^{2} + \frac{b}{m l^{2}} s + \frac{g}{l} - \frac{K}{m l^{2}}}) (\frac{1}{τ_{s} s + 1}) (\frac{- s + \frac{1}{T}}{s + \frac{1}{T}})

(31)

where τ_s is the time constant of the first-order model and T is the time delay. With the transfer function in (31), the real system can be approximated quite well, as shown in Figure 12.

4.3 Pole Placements

Figure 13 shows the root locus of the IPCJ system identified in Figure 12. As shown in the figure, the system is a lightly damped non-minimum phase system.

Figure 13.

Root locus of the IPCJ system identified in Figure 12.

The damping characteristic of the system can be improved by moving the IPCJ poles to more damped poles. The poles of (31) are

P = {\begin{matrix} - ζ w_{n} \pm w_{n} \sqrt{1 - ζ^{2}}, & - \frac{1}{τ_{s}}, & - \frac{1}{T} \end{matrix}}

(32)

where $w_{n} = \sqrt{\frac{g}{l} - \frac{K}{m l^{2}}}$ and $ζ = \frac{b}{2 w_{n} m l^{2}}$ .

Since the poles of the IPCJ dynamics are dominant, we move only those poles. Then, the desired poles can be written as

P_{d e s} = {\begin{matrix} - ζ_{d e s} w_{d e s} \pm w_{d e s} \sqrt{1 - ζ_{d e s}^{2}}, & - \frac{1}{τ_{s}}, & - \frac{1}{T} \end{matrix}}

(33)

where ζ_des and w_des are the desired damping ratio and desired natural frequency, respectively. The state feedback gain K_f can be found by comparing the coefficients of the characteristic equation det(sI-(A-BK_f)) with those of the characteristic equation that has the desired poles as its roots. A block diagram of the damping controller is shown in Figure 14.

Figure 14.

Block diagram of the damping controller.

5. Experiments

5.1 Experimental Setup

For the experiments, we use a life-size humanoid robot, Hubo [16], developed at KAIST. Each of the robot's joints has a local servo controller that uses high-gain PD position control law and controls the joint motor at 1 kHz control frequency. The inverse kinematic control framework runs in an embedded computer and sends position commands to the lower body joints at 200 Hz and to the upper body joints at 100 Hz.

The kinematic and inertial parameters are obtained from a 3D CAD model of Hubo. While the joint position limit is derived explicitly from the mechanical design of the joint, we decided on the joint velocity and acceleration limits experimentally.

5.2 Effect of the Damping Controller

In order to identify the system parameters, we conducted frequency response tests for four cases: x˜z˜-DSP, y˜z˜ -DSP, x˜z˜-SSP, and y˜z˜-SSP. Damping controllers were designed to move the open-loop poles to damped ones such that damping ratios became 0.72.

As shown in Figure 15, we exerted external forces on the humanoid robot and measured ZMP. Figure 16 and Figure 17 show the results. From these figures, it can be verified that the damping controller damps out the oscillation caused by external force.

Figure 15.

Exerting external force on the humanoid robot to verify the effectiveness of the proposed damping controller. (a) DSP (b) SSP

Figure 16.

Time response of measured ZMP in DSP.

Figure 17.

Time response of measured ZMP in SSP.

5.3 Imitating Captured Motion by a Humanoid Robot

In this experiment, a captured human motion was imitated by the humanoid robot in order to verify the proposed inverse kinematic control framework. We used captured motion data given as joint trajectories for the arms and attitudes for the pelvis. Tracking of the captured motion data was treated as one task among others, e.g., positioning of COM for balancing. For the given joint constraints, a solution satisfying the constraints and tasks in order of priority was obtained using the proposed methods.

The captured motion data contain stepping but we used our own walking pattern for the lower body motion. Because of the kinematic and dynamic differences between humanoids and humans, the imitation of stepping motion requires more complicated mapping of the captured motion and thus is beyond the scope of this paper.

The tasks for imitating the captured motion are shown in Table 1.

Table 1.

Tasks for imitating a captured motion.

Priority	Tasks	DOF
1 (Highest)	Tracking of foot trajectories.	12
2	Tracking of COM trajectories (horizontal plane).	2
3	Tracking of pelvis attitude trajectories.	3
4	Tracking of a pelvis height trajectory.	1
5 (Lowest)	Tracking of the upper body joint trajectories (arms, waist and neck).	16
	Total	34

In order to verify the IKTC algorithm, we narrowed the range limits of the left-knee joint (LKN) and the left-hip-roll (LHR) joint compared to the ordinary ranges. This caused corrections of the lower priority tasks, pelvis height, and attitude tasks.

As shown in Figures 18 and 19, the LKN and LHR joints did not violate constraints. Figure 20 shows task corrections of the pelvis height and pelvis attitude tasks. Figure 21 shows a comparison of the captured motion and the motion imitated by the robot.

Figure 18.

Trajectories of q_LKN, q̇_LKN and q̈_LKN in Experiment 5.3.

Figure 19.

Trajectories of q_LHR, q_LHR and q_LHR in the experiment 5.3.

Figure 20.

Desired task trajectories and their corrected trajectories in Experiment 5.3. Top: pelvis height task; bottom: pelvis attitude task.

Figure 21.

Captured human motion and imitated motion by a humanoid robot in Experiment 5.3.

6. Conclusion

In this paper, we proposed an inverse kinematic control framework for a position controlled humanoid robot with bounded joint range, velocity, and acceleration limits.

The proposed framework comprises IKTC and a damping controller.

In the IKTC algorithm, an inverse kinematics solution that considers task priority is found. The three different kinds of joint constraints are taken into account as second-order inequality constraints and are satisfied by optimal task corrections. The efficiency of the algorithm is attained by reducing the number of QPs to one.

The damping controller reduces the instability of the stiff position controlled humanoid robot. It uses IPCJ in order to take into account the compliance around the foot; this process is applicable not only in SSP but also in DSP by using ZMP measurement.

The validity of the proposed methods was shown through experiments.

References

Antonelli

Chiaverini

Fusco

, (2003) “A new on-line algorithm for inverse kinematics of robot manipulators ensuring path tracking capability under joint limits”, IEEE Trans. on Robotics and Automation, Vol. 19, No. 1, pp. 162–167.

Chiacchio

Chiaverini

, (1995) “Coping with joint velocity limits in first-order inverse kinematics algorithms: Analysis and real-time implementation”, Robotica, Vol. 13, No. 5, pp. 515–519.

Chan

T. F.

Dubey

R. V.

, (1995) “A weighted least-norm solution based scheme for avoiding joint limits for redundant joint manipulators”, IEEE Trans. on Robotics and Automation, Vol. 11, No. 2, pp. 286–292.

Deo

A. S.

Walker

I. D.

, (1997) “Minimum effort inverse kinematics for redundant manipulators”, IEEE Trans. on Robotics and Automation, Vol. 13, No. 5, pp. 767–775.

Flacco

De Luca

Khatib

, (2012) “Motion control of redundant robots under joint constraints: Saturation in the null space”, Proc. of IEEE Int. Conf. on Robotics and Automation, Saint Paul, MN, pp. 285–292.

Hanafusa

Yoshikawa

Nakamura

, (1981) “Analysis and control of articulated robot with redundancy”, IFAC, 8th Triennial World Congress, Vol. 4, pp. 1927–1932.

Kanoun

Lamiraux

Wieber

P. -B.

, (2011) “Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task”, IEEE Trans. on Robotics, Vol. 27, No. 4, pp. 785–792.

Khatib

, (1986) “Real-time obstacle avoidance for manipulators and mobile robots”, Int. J. of Robotics Research, Vol. 5, No. 1, pp. 90–98.

Kim

J. H.

, (2004) “Walking control of the humanoid robot platform KHR-1 based on torque feedback control”, Proc. IEEE Int. Conf. on Robotics and Automation, New Orleans, LA, pp. 623–628.

10.

Liegeois

, (1977) “Automatic supervisory control of the configuration and behavior of multibody mechanisms”, IEEE Trans. on Systems, Man and Cybernetics, Vol. SMC-7, No. 12, pp. 868–871.

11.

Maciejewski

A. A.

Klein

C. A.

, (1985) “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments”, Int. J. of Robotics Research, Vol. 4, No. 3, pp. 109–117.

12.

Nakamura

Hanafusa

Yoshikawa

, (1987) “Task-priority based redundancy control of robot manipulators”, Int. J. of Robotics Research, Vol. 6, No. 2, pp. 3–15.

13.

Nise

N. S.

, (2000) Control Systems Engineering (3rd Edition), John Wiley & Sons, Inc., New York.

14.

Nocedal

Wright

S. J.

, (2006) Quadratic Programming. In: Numerical Optimization (2nd Edition), Springer, New York.

15.

Park

I. W.

Kim

J. Y.

Lee

J. H.

, (2005) “Mechanical design of humanoid robot platform KHR-3 (KAIST Humanoid Robot - 3: HUBO)”, Proc. IEEE-RAS Int. Conf. on Humanoid Robots, Tsukuba, Japan, pp. 321–326.

16.

Siciliano

, (1990) “A closed-loop inverse kinematic scheme for on-line joint-based robot control”, Robotica, Vol. 8, pp. 231–243.

17.

Siciliano

Slotine

J. -J. E.

, (1991) “A general framework for managing multiple tasks in highly redundant robotic systems”, Proc. of Int. Conf. on Advanced Robotics, ‘Robots in Unstructured Environments’, Pisa, Italy, pp. 1211–1216.

18.

Wampler

C. W.

II , (1986) “Manipulator inverse kinematic solutions based on vector formulation and damped least-squares methods”, IEEE Trans. on Sys., Man and Cyb., Vol. SMC-16, No. 1, pp. 93–101.

19.

Vukobratovic

Borovac

, (2004) “Zero-Moment Point – Thirty five years of its life”, Int. J. of Humanoid Robotics, Vol. 1, No. 1, pp. 157–173.