Application and Research of Humanoid Robot Based on Second-Order Cone Programming

Abstract

It is an extremely complex process of controlling walking motion for humanoid robot, and its dynamics model has many rich features. The article puts forward a kind of optimization design method on any time humanoid robot walking movement. Firstly, this article makes a stability analysis of walking humanoid robot based on the ZMP criterion, at the same time with the design of using a humanoid robot walking movement by the criterion of the error between the expectations and kinetic energy of the weighted kinetic minimum norm make the problem into a second-order cone programming (SOCP) optimization problem, then use the interior point method to solve the kinetic energy of optimization coefficient of the humanoid robot walking movement, and by compares with the LMS design method and genetic algorithms, finally the algorithm is validated in the simulation and experiment, the numerical results are present for illustration.

Keywords

humanoid robot stability analysis motion control second-order cone programming

1. Introduction

A second-order cone method is a kind of optimization design method that can satisfy many targets [1]. For norm standards, including single norm standards and mixed norm standards, they can both be realized by the secondorder cone method [2]. What's more, the second-order cone method can also realize the robust filter [3]. The major concept of the second-order cone method is to change the objective function in the optimization problem into the restraint function by introducing variables. Then combine the new objective function variables with the original optimize-needed variables into new optimization variables. The original second-order cone constraints can be transformed into the second-order cone programming about the new optimization variables. Last, as long as the objective function and the restraint function in the optimization problem can be expressed to the second-order cone form, the optimization problem can be solved by the second-order cone programming method. Lobo Miguel Sousa etc[4] presented a second-order cone programming which is can be efficiently Applicated in many fields.

The second-order cone methods have been used in robots on several fields, but the applied range of it is not very vast. Gao Chao etc examined the problem of extrinsic calibration of multiple LIDARs on a mobile vehicle platform[5]. Derenick Jason C etc[6–7] presented an optimization framework for target tracking with mobile robot teams and presented a discrete-time optimization framework for target tracking with multi-agent systems. Then Derenick Jason C etc[8] investigated convex optimization strategies for coordinating a large-scale team of fully actuated mobile robots. Spletzer John R etc[9] considered the task of repositioning a formation of robots to a new shape while minimizing either the maximum distance that any robot travels, or the total distance traveled by the formation. Verscheure Diederik etc[10–11] focused on time-optimal path tracking, a subproblem in time-optimal motion planning of robot systems and focused on time-optimal and time-energy optimal path tracking, which are subproblems in optimal motion planning of robot systems. Chakraborty Nilanjan etc[12] presented algorithms that allows a robot to decide when it is feasible for it to move to a desired point by adjusting its own positions (and the positions of some other robots in the network), while maintaining all the network connectivity constraints. Khemchandani Reshma etc[13] treated the kernel selection problem as an optimization problem over the convex set of finitely many basic kernels, and formulate it as a second order cone programming (SOCP) problem.

The second-order cone methods have been also widely used in filter design optimization problem and beam-forming device optimization problems prior to be used in robots, and obtain a good actual effect. Y. Q. Bai, M. EL GHami and C. Roos discussed respectively the dual interior point algorithm [14–15] to solve the second-order cone optimization and linear programming problems by using the function of kernel. Widrow etc [16] use the LMS self-adaptive method, which makes the design filter frequency response convergence to expect response. Yuan etc[17] mentioned a formulation which leads to an optimization problem to design SMF, which is solved efficiently in a second-order cone programming (SOCP) framework by an interior point implementation. YANG Yi-xin etc [18] improve the LMS self-adaptive method, and change the value of each frequency index on the value factor automatically in the FIR filters self-adaptive process. Finally, realize the optimal value of the cost factor, this paper to draw upon this method, introducing it to the humanoid robot walking control optimization, and make a comparison.

Humanoid robot is designed on the Angle of bionics. Comparing with the four feet walking robot and wheeled robot, humanoid robot's centre of gravity is higher and bottom supporting surface is smaller in order to make it two feet touchdown. These characteristics make itself natural compose an unstable structure. Comparing with the wheeled robot, humanoid robot has advantages of flexible motion, activity convenient and so on. It's sure to play an important role in the future of the human society. But the increase of labor and the expansion of activity scope will greatly increase the danger of humanoid robot's action, especially the knocking and the falling are inevitable when working in uncertain environment. At the same time, with the unceasing development of humanoid robot technology, the size of the robot will continue to increase. For these bigger humanoid robots in size, if walking instability, the consequences of falling are always destructive. Based on the above, the control technology and the optimization design model of humanoid robot's walking and falling are of great practical significance. This article uses the second-order cone optimization algorithm to control and optimize the process of humanoid robot's walking effectively. And it puts forward to the optimization algorithm of robot walking control.

2. Second-order cone programming

Second-order cone programming is a subset of the convex programming problem. It makes a linear function to minimize in the condition of satisfying the a group of second-order cone constraints and linear equation constraints. It is expressed as follows:

\underset{y}{min b^{T} y}

(1a)

subject to ∥ A_{i} y + b_{i} ∥ \leq c_{y}^{T} y + d_{i}, i = 1, 2, \dots, I

(1b)

F y = g

(1c)

Where b ∈ C^α+1, y ∈ C^α+1, A _i ∈ C^(α_i-1)xα, b _i ∈ C^(α_i-1)x1, c _i ∈ C^(α_i-1)x1, c ^T_i y ∈ R, d _i ∈ R, F ∈ C^gxα, g ∈ C^gx1, ‖·‖ stand for Euclid norm, (·)^T stand for the transpose, C stand for plural sets, R stand for real sets. Each constraint in (1b) can be expressed as second-order cone.

[\begin{array}{l} c_{i}^{T} \\ A_{i} \end{array}] y + [\begin{array}{l} d_{i} \\ b_{i} \end{array}] \in S O C_{i}^{α_{i}}

(2)

Where SOC^α_i_i is the second-order cone of Space C^{α
_i}. It is defined as follows:

S O C_{i}^{α_{i}} \overset{Δ}{=} {[\begin{array}{l} t \\ x \end{array}] | t \in R, x \in C^{(α_{i} - 1) \times 1}, ‖ x ‖ \leq t}

(3)

The geometrical meaning of second-order cone optimization is to find the optimal point satisfying the objective function of minimizing in three-dimensional second-order cone.

The equality constraints in (1c) can be expressed as zero cone:

g - Fy \in {0}^{g}

(4)

Where the zero cone {0}^g is defined as follows:

{0}^{g} \overset{Δ}{=} {x | x \in C^{g \times 1}, x = 0}

(5)

From (1) we can see that the linear programming (linear inequality constraints) and convex quadratic programming are the exceptions of second-order cone programming. On the other hand, the second-order cone programming itself also the subset of semi-definite programming. Because the second-order cone constraints in (1b) can be expressed as the linear matrix inequality

[\begin{array}{l} (c_{i}^{T} y + d_{i}) I \\ {(A_{i} y + b_{i})}^{T} \end{array} \begin{array}{l} A_{i} y + b_{i} \\ c_{i}^{T} y + d_{i} \end{array}] ≻ 0

(6)

Where I is the unit matrix of appropriate dimension, “≻” stands for the matrix semipositone.

3. Dynamics model of humanoid robot

It's enough to adopt 2D inverted pendulum or three-link bipedal model, when studying the posture and simple sports such as swing. But if you want a better understanding of humanoid robot walking, such model above is far from enough. In the paper, a seven of connecting rod humanoid robot model is built on the longitudinal plane. In this model, the humanoid robot's any part of the body (here first not consider his arms and head) is composed by rigid connecting rod, connecting the rods by joints. Controlling the joint rotation can promote the movement of connecting rod. Seven connecting rods stand for two feet, two little legs, two legs and an upper body. For convenience, this model takes the model longitudinal vertical plane named z - x coordinate motion model as the example, not considering the situation of the model horizontal vertical plane named z - y coordinate model. Specific model is shown in Figure 1, and assumed that each connecting rod's quality is concentrated on the centre, θ_i(i=1…7) is the included angel of the connecting rod and the ground vertical line.

Humanoid robots walk can be seen as a process in which a single leg support and two legs support alternate execute. Two legs support takes much shorter time in the whole walking cycle, so it can be ignored, and only consider single leg support. In robot single leg support cycle, simplify it into multistage inverted pendulum. Taking the left support legs toe of the robot in the picture as the origin, we can get expressions of each robot connecting rod's centre of gravity coordinate ( x _i, z _i)_(i=1…7) through geometrical relation.

Figure 1.

Model of 7 linkage humanoid model

x_{i} = \sum_{1}^{i} S θ_{i} l_{i}

(7)

z_{i} = \sum_{1}^{i} C θ_{i} r_{i} (z_{i} = \sum_{1}^{i} C θ_{i} l_{i})

(8)

Where S is the abbreviation of sin, and C is the abbreviation of cos. r_i(i = 1…7) stands for the distance between the centre of mass of the joint and the connection point of joint i-1 and joint i. l_i(i = 1…7) stands for the length of joint i. According to the formula (10) and (11), we can get the kinetic energy of robot T:

T = \sum_{i = 1}^{7} \frac{1}{2} m_{i} ({\dot{x}}_{i}^{2} + {\dot{z}}_{i}^{2}) + \frac{1}{2} I_{i} {\dot{q}}_{i}^{2} = \frac{1}{2} {\dot{q}}^{T} J \dot{q}

(9)

Where θ_i and θ̇_i separately stands for the rotation angle and speed of joint i of the robot shown in Figure 2-1. I_i is the moment of inertia of joint I, and m_i is the quality of joint i. J(θ) = [L_ij cos(θ_i - θ_j)]_ixj∈7×7 is quality inertial matrix. q_i(i = 1,…, 7) is the Angle between the Adjacent rod i and i-1, exactly the rotational angle q_i of rotation joint.

4. Stability analysis of humanoid robot

The application point which the resultant force F of ground counterforce passes through is the moment zero point, abbreviation ZMP. As long as the robot moves, with the ZMP point inside the support of polygon, the robot is thought to keep its stability. To use ZMP criterion, first assume that the feet of robot fixed on the ground, and keeping contact with the ground surface, and the robot posture, absolute angular velocity and linear velocity can be measured.

The ZMP of double sufficient exercise for humanoid robot can be obtained from the following formula:

p_{x} = \frac{M g x + p_{z} {\dot{P}}_{x} - {\dot{L}}_{y}}{M g + {\dot{P}}_{z}} & p_{y} = \frac{M g y + p_{z} {\dot{P}}_{y} - {\dot{L}}_{x}}{M g + {\dot{P}}_{z}}

(10)

Where Ṗ = Mg + f is the relation between kinetic momentum and the ground forces, and λ = c x Mg + τ is relation between momentum and the torque of ground forces. If the inertia tensor of each robot connecting rod around its centroid is ignored, we can get the expression of the simplified ZMP:

p_{x} = \frac{\sum_{i = 1}^{N} {({\ddot{z}}_{i} + g) x_{i} - (z_{i} - p_{z}) {\ddot{x}}_{i}}}{\sum_{i = 1}^{N} ({\ddot{z}}_{i} + g)} & p_{y} = \frac{\sum_{i = 1}^{N} {({\ddot{z}}_{i} + g) y_{i} - (z_{i} - p_{z}) {\ddot{y}}_{i}}}{\sum_{i = 1}^{N} ({\ddot{z}}_{i} + g)}

(11)

5. Optimal control algorithm for walking

Suppose the rotation angle response of humanoid robot rotational joint is q = [q̇(1), q̇(2), …, q̇(L)]^T, its kinetic energy at a movement moment in the humanoid robot can be expressed as follows:

T (t) = \sum_{l = 1}^{L} q (l) p (l) = p^{T} (l) q

(12)

Where p (t) = q̇ ^T J (ij).

Suppose the sampling points of time are respectively f_k ∈ F(k = 1, 2, …, K), these sampling points have uniform or non-uniform intervals. Solving humanoid robots walking optimization problem is to make the following error weighted norm minimum.

{\sum_{k = 1}^{K} λ_{k} {| T_{d} (t_{k}) - T (t_{k}) |}^{p}}^{\frac{1}{p}}

(13)

Where T_d(t_k) is the expectation response of the optimization design in time t_k, is the nonnegative weighting coefficient, which is used to adjust fitting closely degree of different frequencies. Typically, error norm generally take L₁, L₂ or norm L_∞, exactly p = 1,2, or, ∞. The filter design problem of these three norm standards can be expressed separately as follows:

\min_{q} \sum_{k = 1}^{K} (λ_{k} | T_{d} (t_{k}) - p^{T} (t_{k}) q |)

(14)

\min_{q} \sum_{k = 1}^{K} λ_{k} {| T_{d} (t_{k}) - p^{T} (t_{k}) q |}^{2}

(15)

\min_{q} \max_{k} (λ_{k} | T_{d} (t_{k}) - p^{T} (t_{k}) q |)

(16)

Take the L₂ norm standard shown in (15) as an example.

We can solve it by changing the optimization problem into a second-order cone programming problem shown in (17). Introducing a set nonnegative variables ɛ_k(k = 1, 2, …, K), (15) can be changed as:

\begin{array}{c} \min_{q} \sum_{k = 1}^{K} (λ_{k} ε_{k}), subject to {| T_{d} (t_{k}) - p^{T} (t_{k}) q |}^{2} \leq ε_{k} \\ k = 1, 2, \dots, K \end{array}

(17)

To the second inequality constraints in (17):

\begin{array}{c} {| T_{d} (t_{k}) - p^{T} (t_{k}) q |}^{2} \leq ε_{k} \\ \Leftrightarrow {| 2 T_{d} (t_{k}) - 2 p^{T} (t_{k}) q |}^{2} + 1 + ε_{k}^{2} - 2 ε_{k} \leq 1 + ε_{k}^{2} + 2 ε_{k} \\ \Leftrightarrow {‖ \begin{matrix} 2 T_{d} (t_{k}) - 2 p^{T} (t_{k}) q \\ ε_{k} - 1 \end{matrix} ‖}^{2} \leq {(ε_{k} + 1)}^{2} \\ \Leftrightarrow ‖ \begin{matrix} 2 T_{d} (t_{k}) - 2 p^{T} (t_{k}) q \\ ε_{k} - 1 \end{matrix} ‖ \leq ε_{k} + 1 \end{array}

(18)

Define

$y = {[ɛ_{1}, ɛ_{2}, \dots, ɛ_{K}, q^{T}]}^{T}$ and $b = {[λ_{1}, λ_{2} \dots, λ_{K}, 0_{1 x L}]}^{T}$ .

satisfying $b^{T} y = \sum_{k = 1}^{K} (λ_{k} ɛ_{k})$ , where 0_1xL stands for zero vector of 1 x L dimension, (17) turns into:

min b ^T y , subject to

‖ [\begin{matrix} 2 T_{d} (t_{k}) \\ - 1 \end{matrix}] - [\begin{matrix} 0_{1 \times K} & 2 p^{T} (t_{k}) \\ - m^{T} (k) & 0_{1 \times L} \end{matrix}] y ‖ \leq 1 + [\begin{matrix} m^{T} (k) & 0_{1 \times L} \end{matrix}] y, k = 1, 2, \dots, K

(19)

Where $m (k) = {[m_{1}, m_{2}, \dots, m_{n}, \dots, m_{K}]}^{T}, m_{i} = {\begin{matrix} 0, n \neq k, \\ 1, n = k . \end{matrix}$

Till now the norm standards humanoid robots walking optimization problems (12) have turned into the restraint function algorithm in order to solve.

6. Simulation and experiment

Using respectively LMS self-adaptive method and second-order cone programming optimization design method in this article, and the Matlab simulation helps realize humanoid robot walking movement optimization design at any arbitrary time node. It also helps examine the performance of humanoid robots walking movement optimization design model designed in this article, and make the comparison of iteration error effects and displacement error effect respectively between this method and GA.

Select test time for 0.8 s, and in this period the walking research of robots is enough to satisfy the stability requirement for the instantaneous walking. At the same time take time node 0.1 s step, and assumed that humanoid robot movement speed is v = 2m / s , humanoid robot quality is m, and the robot motion kinetic energy expectations should be 2 Kg.J . The simulation results are as follows. Where Fig. 2(a) is the simulation of the robot motion kinetic energy, and Fig. 2(b) is the comparison of walking optimization design in delay aspects between the two methods.

Figure 2(a)

Energy of motion changing with time

In order to prove the superiority of the second-order cone optimization method in optimization design, we make a comparison of iteration error effects and displacement error effect respectively between this method and GA. Fig. 3 is the comparison figure of second-order cone optimization method and GA at iteration error effects. Where the initial population of GA is 12, dimension is 7, taking the mutation probability 0.6%, number of iterations is 500.Fig. 4 is the comparison figure of second-order cone optimization method and GA at displacement error effect. Make the humanoid robot walk along the marked straight line. The test distance is 5m. Make the optimization to robot gait respectively by second-order cone optimization algorithm and genetic algorithm, and test the error of the deviation from the marked line.

Figure 2(b)

Error on delay

From Fig. 3, the convergence speed of the SOCP iteration error effect is faster than GA at the start, and in about iteration 160 it will begin to tend to the optimal value. But the GA shocks significantly, and begins to tend steady in about 280 steps.

Figure 3.

Effort of iteration error with GA

Figure 4.

Effort of displacement error with GA

In the process of humanoid robot walking straight along the marked 5m line, because the ground is not smooth etc subjective factor, and robot has no vision navigation guidance, it is unavoidable to deviate from its specified orbit. From Fig. 4, the displacement error effect of SOCP is better than GA optimization algorithm.

Figure 5.

Simulation of walking on slop by Matlab

In case of offline, use Matlab to simulate the control method, and make the experiment on real robot NAO. In Fig. 5, one step simulation of walking on slop is present, and in Fig. 6 and Fig. 7, experimental screenshot for walking on slop and step are proposed.

Figure 6.

Screenshots of the process of walking up and down a slope of robot NAO

7. Conclusion

The model of a seven-connecting rob humanoid robot is built in the longitudinal plane, constructing the process of humanoid robot walking into a multistage inverted pendulum form. In view of the complexity of this optimal control problem is great, this article use second-order cone optimization programming design to change the optimal control problem into second-order cone optimization problem, and use the Matlab to make the simulation test, comparing with the LMS method at the same time. According to the simulation, this method is very effective for complex mathematical optimization problems containing constraints. And when piecewise number diminishes, this method can realize online optimization. Experimental results show that this control method can effectively control humanoid robot walking process and ensure its walking stability.

Figure 7.

Screenshots of the process of walking up and down a step of robot NAO

8. Acknowledgments

Project 61075077 supported by National Natural Science Foundation of China. Project Supported by Natural Science Foundation of Heilongjiang Province of China.

References

Lobo

Vandenberghe

Boyed

. Applications of second-order cone programming. Linear Algebra Application, 1998, 284(1∼3): 193–228

Yan

S F

Y L

. A unified framework for designing FIR filters with arbitrary magnitude and phase response. Digital Signal Processing 2004, 14(6): 510–522.

Hui-Jun

Gao

Chang-Hong

Wang

. Uncertain discrete-time systems of robust l2 - 1 ∞ and h-infinity filter new methods. Chinese science, E, 2003, 33(8): 695–706.

Lobo

Miguel Sousa

Vandenberghe

Lieven

Boyd

Stephen

Lebret

Herve

, Linear Algebra and Its Applications, v 284, n 1–3, p 193–228, Nov 15 1998.

Gao

Chao

Spletzer

John R

, 2010 IEEE International Conference on Robotics and Automation, ICRA 2010, Proceedings - IEEE International Conference on Robotics and Automation, p 279–284, 2010.

Derenick

Jason C

Spletzer

John R

Hsieh

M. Ani

. Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2007. EE International Conference on Intelligent Robots and Systems, p 3422–3428, 2007,.

Derenick

Jason

Spletzer

John

Hsieh

Ani

, Journal of Intelligent and Robotic Systems: Theory and Applications, v 56, n 1–2, p 47–67, September 2009.

Derenick

Jason C

Spletzer

John R

, IEEE Transactions on Robotics, v 23, n 6, p 1252–1259, December 2007.

Spletzer

John R

Fierro

Rafael

, Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Proceedings - IEEE International Conference on Robotics and Automation, v 2005, p 742–747, 2005.

10.

Verscheure

Diederik

Demeulenaere

Bram

Swevers

Jan

De Schutter

Joris

Diehl

Moritz

, IEEE Transactions on Automatic Control, v 54, n 10, p 2318–2327, 2009.

11.

Verscheure

Diederik

Demeulenaere

Bram

Swevers

Jan

De Schutter

Joris

Diehl

Moritz

, AMC'08 - 10th International Workshop on Advanced Motion Control, Proceedings, International Workshop on Advanced Motion Control, AMC, v 1, p 727–732, 2008.

12.

Chakraborty

Nilanjan

Sycara

Katia

, 2010 IEEE International Conference on Robotics and Automation, ICRA 2010, Proceedings - IEEE International Conference on Robotics and Automation, p 5484–5489, 2010.

13.

Khemchandani Reshma Jayadeva Chandra Suresh , European Journal of Operational Research, v 203, n 3, p 692–697, June 16, 2010.

14.

Bai

Y. Q.

Qang

G Q

Roos

A new primal-dual interior-point algorithm for second-order cone optimization[EB/OL]. http://www.optinization-onlineorg/DB-FILE/2004/11/1001.pdf. November 14, 2004.

15.

Bai

Y. Q.

Ghamiand

M Ei

Roos

A comparative study of kernel functions for primal-dual interior-point algorithms in linear optimization [J]. SMA Journalon Optimization, 2004 15(1): 101–128.

16.

Widrow

Steams

S. D.

Adaptive Signal Processing [M]. Englewood Cliffs, NJ:Prentice-Hall, 1985.

17.

Zhentao

Yuan

Weidong

Wenxian

, Proceedings of the 2008 IEEE International Conference on Information and Automation, ICIA 2008, Proceedings of the 2008 IEEE International Conference on Information and Automation, ICIA 2008, p 1312–1316, 2008.

18.

Xin-Yi

Yang

Chao

Sun

. An improved FIR digital filters adaptive design method [J]. Journal of northwestern polytechnical university, 2002, 20(4): 554–558.