Scholarly article on topic 'The Design and Control of a Bipedal Robot with Sensory Feedback'

The Design and Control of a Bipedal Robot with Sensory Feedback Academic research paper on "Mechanical engineering"

0
0
Share paper
Keywords
{""}

Academic research paper on topic "The Design and Control of a Bipedal Robot with Sensory Feedback"

INTECH

open science | open minds

OPEN Vy ACCESS ARTICLE

International Journal of Advanced Robotic Systems

The Design and Control of a Bipedal Robot with Sensory Feedback

Regular Paper

Teck-Chew Wee1'*, Alessandro Astolfi1 and Xie Ming2

1 Department of Electrical and Electronic Engineering, Imperial College London, UK

2 School of Mechanical and Aerospace Engineering, Nanyang Technological University, Singapore * Corresponding author E-mail: t.weel 0@imperial.ac.uk

Received 18 Dec 2012; Accepted 19 Apr 2013 DOI: 10.5772/56572

© 2013 Wee et al.; licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract A stable walking motion requires effective gait balancing and robust posture correction algorithms. However, to develop and implement such intelligent motion algorithms remains a challenging task for researchers. Effective sensory feedback for stable posture control is essential for bipedal locomotion. In order to minimize the modelling errors and disturbances, this paper presents an effective sensory system and an alternative approach in generating a stable Centre-of-Mass (CoM) trajectory by using an observer-based augmented model predictive control technique with sensory feedback. The proposed approach is used to apply an Augmented Model Predictive Control (AMPC) algorithm with an on-line time shift and to look ahead to process future data to optimize a control signal by minimizing the cost function so that the system is able to track the desired Zero Moment Point (ZMP) as closely as possible, and at the same time to limit the motion jerk. The robot's feet are fitted with force sensors to measure the contact force's location. An observer is also implemented into the system.

Keywords Humanoid Robot, Walking Gait, Augmented Model Predictive Control

1. Introduction

The development of bipedal humanoid robots began more than 30 years ago. Despite numerous research efforts, controlling a bipedal robot is still an extremely challenging task in terms of adaptability, robustness and stability. The most common approaches are the Linear Inverted Pendulum Model (LIPM) and the Zero Moment Point (ZMP) method as in [1,2]. The problem with the LIPM method is that it is a non-minimum phase system [3], which produces a undershoot response to a step input. In order to compensate for this undesired effect, a new observer-based Augmented Model Predictive Control (AMPC) method is proposed in this paper. Together with the sensory feedback system, this method reduces the jerk produced by the ground landing impact forces and is also able to improve the overall system performance of ZMP tracking under external disturbances. The effectiveness of the proposed scheme is demonstrated in a simulation and in comparison with the preview control method. Section two explains the robot unique mechanical structure. The robot is designed with a lightweight material and can be correctly modelled like a 'point-mass' system. Section three presents how the sensory system is deployed to enhance the effectiveness

of the feedback system. Section four provides an overview of the robot's electronics control architecture and the various balance control subsystems. Section five gives a detailed implementation of the proposed AMPC control approach. Section six concludes our proposed methods and recommends future work.

2. Robot Mechanical Structure

The TPinokio robot is approximately 1.5m tall and weighs approximately 45kg. It has more than twenty-five degrees of freedom (DoF) as depicted in Figure 1. It is constructed from lightweight aluminium material, as such, the joint-link inertia is negligible. All the components are housed in a compact-joint housing mechanism. It has a simple kinematic structure and it can be modelled as a simplified 'point-mass' system, as in [4]. The robot's feet are fitted with force sensors to measure the ZMP location. The measured ZMP value is used to determine the system tracking error and to provide a feedback signal to an observer [5] in order to improve the system stability.

Figure 1. TPinokio design and stick-diagram) 2.1 Modular Joint Design

In order to accurately model the robot as a point-mass system, all the joints are designed with lightweight aluminium and assembled in a compact housing, as depicted in Figure 2 and Figure 3.

Harmonic gear

Pulley &

System

Figure 3. Compact modular design of the joint 3. Robot Sensory System

In order to improve the stability and enhance the robot walking posture, an intelligent sensory system is deployed at strategic joint positions to provide sensory feedback.

3.1 Foot force sensor

Each foot sole and toe are fitted with four load-cell sensors, a total of eight load-cell sensors per foot, as depicted in Figure 4.

load-cell with rubber pad

Figure 4. Load-cell sensors and its mounting location.

The value of the sensor's reading is fed through a filter/amplifier to remove noises. The data is then sent to the controller for processing.

During the single support and double support phases, the ZMP can be calculated as depicted in Figure 5a and 5b and Equation (2.1) and (2.2), respectively.

r _ Ef=i Vixfi . . _ Yf=l Viyfi (2 1)

rzmv Yr f' ' ^zmV Yrf' /

r _ VLxfL + VRxfR . . _ PLyfL + PRyfR (2 2)

XzmP fL+fR ' YzmV fL+fR ( )

(a) Pelvis (b) hip

Figure 2. Modular parts

(c) Knee (d) Ankle and foot

Figure 5a. Force/ZMP coordinates in single foot support

Figure 5b. Force/ZMP coordinates in double support phase

3.2 Inertia and vision sensor

The robot guidance and navigation system includes a nine-axis gyro sensor that provides orientation and angular motion feedback to the stabilizer controller. The path search and obstacle avoidance controller receives feedback from the 270o laser scanner and the USB CAM. As depicted in Figure 6 (a), (b) and (c).

Figure 6. (a) 9-axis gyro sensor, (b) Laser scanner, (c) USB CAM 3.3 Position sensor

Each individual joint of the robot is fitted with an absolute encoder and an optical sensor, as depicted in Figure 7. The optical sensor is for initialization; the absolute encoder is used for real time tracking of the joint's position.

Figure 7. Absolute position sensor in all joints.

3.4 Absolute ground sensor

Two tri-axis tilt sensors are fitted at the robot ankle joint to measure the ankle's pitch and the roll angles, as depicted in Figure 8. The function of these sensors is to sense the x-y tilt angles of the feet and ground contact point. This is to provide feedback from the ground conditions to the robot stability controller, so that the controller can adjust the posture of the robot accordingly and to react to any external forces to keep the robot in a stable condition to prevent it falling.

Figure 8. Tilt sensor at ankle joint

4. Robot Control Architecture 4.1 Control electronics

The main communication bus for the robot control electronics and sensory system is the CANbus. The secondary buses are the USB and RS232, as depicted in Figure 9.

Figure 9. Control electronics and sensory system architecture

4.2 Balancing Control with sensors feedback

During the walk cycle, the hip/pelvis link of the robot may not always maintain in a horizontal position as depicted in Figure 10. This is due to gravitational vibration and other structural mechanism problems. As a result, the swing leg is prone to collide with the stance leg. A nine-axis IMU Figure 6(a), is mounted on to the pelvis position to measure the robot's roll/pitch/yaw motion and two three-axis tilt sensors are mounted on the robot ankle joint to measure the floor inclination during walking and when the swing foot initially touches down on the ground surface. The sensor's reading will compensate for the ground condition error accordingly, as shown in Figure 10 and Figure 11. [6].

Collision avoidance control:

The compensation angle for right hip roll, where kr is the gain:

A0rl = kr\eroll\

K = 0rl ± №rl

The compensation angle for left hip roll, where kt is the gain:

A0n =kl\8ron\

\ = 0n ± A0n

= , r kri Lr — Lr H (kr H

05=0'5+tan-i(^)

5. Robot Modelling 5.1 Kinematics model

The forward kinematics of the robot is defined by applying the Denavit-Hartenberg (D-H) convention method [7], as depicted in Figure 12.

Figure 10. Frontal roll angle compensation of swing leg Slope balancing control:

The slope compensator for the sagittal plane and frontal plane are as depicted in Figure 11.

Figure 11. Sensor feedback compensation on non-planar surface For the sagittal plane, where k is the gain:

8$ = 8$± k\9pltch\

For the frontal plane, where kp and kr are the gain:

Li=L'i + (kl + ]k\0rou

cos8l sin8l 0 —al

—sin8lcosal cos8lcosal sinal —dlsinal

sin8lcosal —cos8lsinal cosal —dlcosal

. 0 0 0 1

xi-l Vi-1 Zl-1 1

Figure 12. TPinokio forward kinematics D-H model

The inverse kinematics of the robot can be solved by searching for the value of the joint q by applying the Newton-Raphson iteration algorithm method [7].

q(i+i) = qm + j-içqm)ôT(qW)

where q is the joint angular position and J is the Jacobian matrix.

Inverse kinematics iteration technique [7].

1. Set the initial counter = 0.

2. Find or guess an initial estimate q(°\

3. Calculate the residue ST(q^-1^) = J^q^Sq^.

If ever element T(q^) of or its norm ||T(q'-'-')|| is less than a tolerance, ||T(qW)|| < e then terminate the iteration. The qW is the desired solution. 5. Set i = i + 1 and return to step 3.

This concludes the kinematics modelling.

5.2 Dynamics model

Bipedal humanoid robots are locomotion systems with rigid bodies; the required torque command to cause each joint variables q(t) to follow a desired path motion as a function of time can be derived by using the Lagrange equations of motion [8]. The three models at different stages of the walking phases are derived from the general form:

M(q)q + C(q, q)q + G(q) = B(q)U (5.3)

The matrix M is the inertia matrix, C denotes the Coriolis matrix, G is the gravity vector, B is the mapping matrix of the joint torque to the generalized forces and U is the applied torques.

Under-actuated phase

The under-actuated phase occurs when the heel of the stance leg rises from the ground and the robot begins to roll over the toe. Because there is no actuation between the toe and the contact surface, the dynamics of the robot in this phase is equivalent to an n-DoF robot with un-actuated feet. The dynamics are:

Mu(qu)qu + Cu(qu, qu)qu + Gu(qJ = BUUU (5.4)

Where qu = [q1,q2...qn] are the joint angles and Uu = [u1,u2.qn_1] is the vector of torque applied at the joint. Bu is the mapping matrix.

Fully actuated phase

The fully actuated phase is defined as when the stance foot lands flat on the contact surface without slipping and the stance leg ankle acts like an actuated pivot. The dynamics of the robot during the fully actuated phase are equivalent to an n-1 DoF robot without the stance foot and with actuation at the stance ankle [8]. The fully actuated phase dynamics equation is:

Ma(qa)qa + Ca(qa,qa)qa + Ga(qa) = BalUb + Ba2Un_i(5.5)

Where qa = [q1,q2...qn-1] is the ankle configuration, un-1 is the input at the ankle joint and Ub = [u1,u2 un_2] is the vector of inputs applied at the other joints. Ba1 and Ba2 are the mapping matrices.

Double-support (impact) phase

The double-support phase involves the instantaneous impact and reaction forces at the leg ends. It is modelled as the total resultant forces and torques acting on the swing foot at the ankle joint. The dynamics model is:

Md(qd)qd + Cd(qd,qd)qd + Gd(qd) = BdUd + Fext (5.6)

Where Fext denotes the external forces acting on the robot due to the contact between the swing/landing foot and the surface.

By applying an inverse kinematics algorithm, the joint angle q for the desired geometric path of the three reference points (Left-foot X L, right-foot X R and Pelvis X H) of the bipedal robot can be derived, as depicted in Figure 13. The joint actuator torque command can then be calculated by substituting the desired joint angle q into Equation (5.4), (5.5) and (5.6).

(b) Simplified point-mass model Figure 13. Robot geometry and coordinate

5.3 Momentum and motion trajectory

The transformation from the foot-toe/CoM trajectories into an individual link joint trajectory can be carried out with the momentum calculation method [9]. The four

reference frames of the robot are as shown in Figure 13 and are defined as [^ff X H X L X A], which denote the world frame, the pelvis frame, the left-foot frame and the right-foot frame, respectively. The position and orientation of a point in space can be are represented by a translational vector ¡»(3x1) and an orientation matrix R(3x3) as shown:

H = P M

The foot velocity of the two legs can be written as:

[3x3 0

Cj [(Oh! + WW

0 /™][®H]+;iefl;4iefl;

5.4 Joint trajectory generation

The robot's joint trajectory is generated using seven degrees of polynomial; this is to ensure that the all the joint s's start and stop at the same time and its position, velocity, acceleration and jerk parameters are all zero when it reaches the desired angular position. For example, to move the angular position of a joint with a zero jerk start-stop path for q(0) = 10° and q(/) = 45° in T=1s, the unknown coefficient a1,a2, — a7 can be found by solving the following set of polynomial equations:

q(t)-«o+«i(;) +a2(Q2+a3(i)3+a4(i;)4+as(;p)S+a6(i;)6+a7(Q7 (5.13)

Set the eight boundary conditions to:

where _/iegL and /¡eg: are a (6x6) Jacobian matrix of the robot's leg, with rHL = pL — pH and rH: = p: — pH. * is an operator, which translates the vector (3x1) rHL and rH: into a (3x3) skew symmetric matrix rHL and rH:. The linear momentum ?(3X1) and the angular momentum £(3X1) of the CoM of the robot can be calculated by the following formula:

Le/t Leg

;ight Leg

'"113x3 0

4iefl; (5.9)

where m is the robot's total mass, I3X3 is the inertia matrix with respect to the CoM. rHC is the skew symmetric matrix of rHC. M and H are the inertial matrices of the joints. For a stable and fast walking motion, the angular momentum of the CoM is set to zero (i.e., no rotation) and the translation movement of the CoM can be calculated from Equation (5.9). By definition T = mCoM^ and setting X=0 yields:

Hip Le/t Leg Right Leg

m=r;r ^Mdw+KN: (5.10)

For a given leg velocity ([vL, mL ]T, co: ]T), the Hip velocity can be obtained by:

[Oh] = [ml3x3 "<Hc] ([mCoM,] _ [Mi [(H] I 0 j

(lmCoM']_[M;]^-_[M;]4ieSR) (5.11)

Finally, the leg's joints angle can be calculated by:

^iegL =^iegL [coL] —^iegL 0 /^J [®h]

-/iea; [J /¡ea; 0 /™J [

(5.12)

This concludes the robot dynamics modelling.

4(0)-40-10 4(0)-0 4(0) - 0 4(0) - 0 q(l)-qy-45 4(1) - 0 4(1) - 0 4(1)-0 ( )

Solving the coefficients (a0 to «7) with the following matrix:

1 0 0 0 0 0 0 0 «0" 40

0 1 0 0 0 0 0 0 «1 0

0 0 2 0 0 0 0 0 «2 0

0 0 0 6 0 0 0 0 «3 0

1 1 1 1 1 1 1 1 «4 4/

0 1 2 3 4 5 6 7 «s 0

0 0 2 6 12 20 30 42 «6 0

0 0 0 6 24 60 120 210- L«7- 0

The path motion is as shown in Figure 14.

seyéti degree'|iolynomia path trajectory

0 2 0.3 0.4' 0.5 0.B M 0.8 0.9 time(s)

(5.15)

Figure 14. Angular position trajectory of the joint

5.5 Foot trajectory

The basic parameters for a humanoid walking are step-length 5, pelvis height Z, foot lifting height H and frontal-shift F, as depicted in Figure 15.

Figure 15. A typical bipedal walking gait.

For TPinokio, a parabolic trajectory for the foot motion of the swing leg is shown in Figure 16.

x(t) = —S cos(tet)

Z(t) =|[1 —cos(2wt)]

(5.16)

Where S is the stride, H is the maximum foot height, T is the one step period and the stride frequency is = p. A sample plot of = 1s,S = 0.2m,H = 0.1m is shown in Figure 16.

0,1 1 H 05 0

TPinokio swing leg profile

■ D 1 ■¡05 0

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 time(s)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 time(s)

i ; ;

(a) IMP superimposed on robot (b) IMP coordinate

Figure 17. Linear Invert Pendulum Model (LIPM).

The Linear Inverted Pendulum Model (LIPM) equations under constraint control (moving along a defined plane) are given as [3]:

^zmp ^com

> Yzmp Ycom g Ycom (5.17)

The above equations are decoupled, and they can be expressed independently. As such, for the subsequence equations, only the sagittal plan (x-axis) is presented. The frontal plane (y-axis) can be derived with the same procedures. With an extra variable, the system jerk, u is defined as:

= j^.. = ... u xcom Xcom

(5.18)

The LIPM can be expressed in state-space model. Continuous time state-space form:

The ZMP equation for the sagittal plane can be expressed in a continuous time state-space model as:

xcom 0 1 0 xcom 0

xcom = 0 0 1 xcom + 0

-Xcom- 0 0 0 xcom 1

J. № -0.0B -0.0B -0.04 -0.02 0 0.02 0.04 0.0S 0.GB 0.1 step i!(m)

x = \l 0 —

xzmp \ g 1

(5.19)

Figure 16. Foot path trajectory. 5.5 Inverted Pendulum Model

In order to define the relationship of the ZMP and the CoM trajectories, a simplified Linear Inverted Pendulum Model (LIPM) is studied in this paper. During walking, in the leg swing phase, the CoM of the robot may be modelled as a 'point mass' [5] and it is assumed to be connected to the stance foot like an inverted pendulum on a moving base, as depicted in Fig. 17.

Discrete time state-space form:

Equation (5.18) is discretized with intervals of constant length T, yielding:

1 T T2/2' T3/6

xk+l = 0 1 T xk + T2/2

0 0 1 T

= \l 0 1

(5.20)

Equations (5.18) and (5.19) can be used to determine the dynamic behaviour of the LIPM system.

5.6 Characteristics of a Linear Inverted Pendulum Model

The LIPM is a non-minimum phase system; there is an undesired zero at the RHP, as shown in Figure 18 and Figure 19, for the continuous time system and the discrete time system, respectively (with zcom = 0.5m, g = 9.81). The undesired zero appears outside the unit circle. The RHP zero will produce a reverse action undershoot transient response [3] to a step input. Care should be taken to address this initial 'water-bed' effect, as this may cause the bipedal robot to lose balance and fall.

The current time state variables are used to perform the predicting ahead operation [10]. The state-space model of the bipedal robot is presented in this section.

6.1 The Augmented 4Model Predicted Control (AMPC) Model

The proposed AMPC model used in this paper is an augmented state-space model with an embedded integrator. The set of equations (5.20) is re-written into the standard state-space form:

xD(fc + 1) = 4DxD(fc) + BDu(fc)

yD(fc) = CDxD(fc)

Figure 18. Continuous time poles and zeros plot.

Real Axis

Figure 19. Discrete time poles zeros plot.

6. Augmented MPC State-Space Model with Embedded Integrator

Model predictive control (MPC) is designed based on a mathematical model of the plant system. In this paper, a state-space model is used in the tracking system design.

where xD is the discrete-time state variable (CoM), u is the discrete-time control input variable (jerk) and yD is the discrete-time tracked ZMP output. To include an integrator, taking a different operation on both sides of (6.1), yields:

xD(fc + 1) -xD(fc) = 4D(xD(fc) -xD(fc - 1))

+BD(u(fc)-u(fc-1)) (6.2)

The difference in the state variable and the difference in the control variable can be denoted by:

Axm(fc + 1) = xm(fc + 1) - xm(fc) ;

Axm(fc) = xm(fc) - xm(fc - 1) ;

Au(fc + 1) = u(fc) - u(fc - 1) ;

Axm(fc + 1) = 4mAxm(fc) + BmAu(fc). (6.3)

and define a new state variable vector. This yields the augmented state-space model [10]:

x(fc+l)

ÄxD(fc + 1) yD(fc + 1)

x+fl Xfll '[Ä+D (+) "d^D lJLyD(fe)

Äu(fc)

y(fc) = [0 0 0 1]

ÄxD(fc)

. y(fe)

Where 0D = [0 0 0] is the dimension of the state variable. The triplet (4, B, C) is the augmented model.

6.2 Prediction of CoM (the state) and ZMP (the output variable)

The system jerk (the control trajectory) is denoted as:

Au(kt), Au(kt + 1)..... Au(kt + NC-1) (6.5)

Where Nc is the control horizon, used to capture the future control system jerk trajectory. Where Np is the length of the prediction horizon optimization window. The future CoM state variables are predicted for Np samples. Denote the future CoM state variables as:

x(ki + 1\ki),x(ki + 2\kt), ...,x(kt + m\kt), ...,x(kt + Np\ki)(6.6)

Where x(ki+m\ki) is the predicted state variable at (ki +m) with the given current plant information x(kt). The control horizon Nc is chosen to be Nc < Np to the prediction horizon Np. All the predicted variables are in terms of current state variable information x(kt) and the future control parameters Au(ki + j), where j = 0,1,2,... .Nc — 1. Define the following vectors:

Y =\y(ki + 1\ki) y(ki + 2\ki) y(ki + 3\ki). y(ki+ Np\ki)]T AU = [Au(ki) Au(ki + 1) Au(ki + 2) ... Au(ki + Nc — 1)]T(6.7)

where ta is the adjustable look forward time-shift constant. The cost function J is defined as:

J = (RS — Y)T(RS — Y)+AUTRAU (6.9)

Where the first term is linked to the objective of minimizing the errors between the predicted ZMP output and the reference ZMP set point, and the second term reflects the consideration given to the rate of change in the system jerk AU when the objective function J is made as small as possible. The diagonal matrix R = rw^NcxNc(rw ^ 0) is used as a tuning parameter to improve the closed-loop performance. For the case where rw = 0, the goal would be solely to make the ZMP error (RS — Y)T(RS — Y) as small as possible. For the case of a large rw, the cost function is interpreted, as it is carefully consider how large the rate of change in the system jerk AU might be and cautiously reduce the ZMP error. To determine the optimal AU, which minimizes J, substitute Equation (6.8) into (6.9). J is expressed as:

J = (Rs—Fx(ki))T(Rs — Fx(ki)) —2AUTOT(RS — Fx(ki)) + AUT(OTO + R)AU (6.10)

The necessary condition of the minimum J is derived:

The dimensions of Y and U are Np and Nc respectively. By rearranging the parameters into matrix form:

JA- = —2OT(Rs — Fx(kt)) + 2(OTO + R)AU = 0 (6.11)

Y = Fx(ki) + OAU

CB CAB CA2B

CB CAB

CA2 CA2

lCANvi

CANp~NcBl

\-CAnp~1B CAnP~2B CAnP~3B 6.3 ZMP Tracking and Optimization

This optimization process is then translated to find an optimal control vector U such that the error between the input reference ZMP set-point trajectory and the CoM output trajectory is minimized. The vector that contains the set-point data information is written as:

Rs =\ 1 1... 1 ] r(ki + ta) = RSr(ki + ta) (6.8)

By re-arranging Equation (6.11) and substituting (6.8) into (6.11), the optimal solution for the control signal is obtained:

AU = (OTO + R)~1OT(RSr(kl + ta) — Fx(kt)) (6.12)

As such, the optimal solution of the control signal is linked to the ZMP reference set-point signal and the state variable (CoM) by Equation (6.12).

6.4 Simulation

The initial simulation shows good tracking of the desired ZMP and CoM trajectory, as shown in Fig 6. The simulation parameters are given as follows: Nc = 100,Np = 300,ta = 80,rw = 10"5 . The system is able to shift its desired CoM (red cure) to left/right feet during straight walking. Although AMP exhibits an initial undershoot (water-bed effect) due to the inherent characteristics of IPM because of the RHP zero, the CoM output (the desired signal in red) is smooth and it is able to closely follow the reference ZMP (blue waveform) trajectory. Figure 20 shows that the jerk in both the X and Y direction is minimized to zero by the cost function.

uy (system jerk)

0 1 2 3 4 5 6 Time (s)

Figure 20. The jerk reduced to zero.

6.5 Comparison of AMPC with Preview method

With a closer examination of the CoM trajectory produced using the AMPC tracking, as depicted in Figure 20, it can be seen that the CoM curve (in red) moves slowly toward the reference ZMP curve (in blue). This can be interpreted as the controller trying to reduce the jerk and impact force during swing foot landing and cautiously reduce the ZMP error. As a result, the weight of CoM transfers from the supporting foot to the landing foot (new supporting foot) during the double support phase (DSP) is slow and gentle. In comparison with the preview control technique [1], which produces a symmetrical sinusoidal waveform (in green), the AMPC algorithm produces a truncated sinusoidal trajectory during swing foot landing due to its 'soft landing, fast take off' characteristics. The other advantages of using the AMPC model is that the optimization process does not need to solve the Riccati equation and the optimization window can be easily adjusted to fine-tune the system response. Since AMPC uses a recursive iteration method, with continuing improvement in computer hardware and software architecture, this in turn reduces system-processing time and therefore, it is possible to implement it in AMPC real-time.

6.6 Augmented MPC model with noise

One of the key features of AMPC predictive control is the ability to handle constraints in the design. To check the robustness of the AMPC model in tracking the desired ZMP, random noise (±0.05m) is added to the reference ZMP waveform. The simulation results are as depicted in Figure 21. This shows that the AMPC model is still able to produce a good track output in a noisy environment.

An observer may be in a situation where either the state variable x(kt) is too noisy or not measurable. The robot state x(ki) is estimated by an observer of the form:

Figure 21. AMPC and Preview control plot.

Figure 22. AMPC trajectory tracking with noisy input.

6.7 Observer-based Predictive Control

The predictive control law is similar to Equation (6.10), to find AU by minimizing:

J = (RS- Fx(ki))T(Rsr(ki) - Fx(k0) - 2AUT0T(RS - Fx(kd) +AUT(0T0 + R)AU (6.14)

The optimal solution is obtained as:

x(ki + !) = Ax(ki) + BAuikO + Koh{y{ki) - CxikO) (6.13)

AU = i0T0 + R)~10T(RS - Fx(ki)) (6.15)

The optimal solution for Au(k{) at time ki, from (6.15) is:

Au(kt) = Kyr(ki) - KXx(ki) (6.16)

The closed-loop characteristic equations and the Eigenvalues of the closed-loop system are obtained as follows:

x(k + 1) = Ax(k) + BAu(k) = Ax(k) + BKyr(k) - BKXx(k) (6.17)

The close loop observer error equation is:

x(k + 1) = (A- KobC)x(k) (6.18)

Where x(k) = x(k) - x(k), (6.17) can be rewritten as:

x(k + 1) = (A - BKX)x(k) - BKXx(k) - BKyr(k) (6.19)

Combining (6.18) and (6.19) leads to:

\x(k + 1)] \A-KobC 0nxn ]\Z(k)] + \°nx,n],, W6 20) [x(k + 1)J = I -BKX A - BKX\ [x(k)\ + [BKy\r(k) (6.20)

The characteristic equation of the close-loop state space system is obtained as:

(6.21)

-BKX A - BKX det(XI -(A- KobC)) det (XI -(A- BKX)) = 0 (6.22)

This means that the robot closed-loop model and the state estimator have two independent characteristic equations:

det(XI -(A- KobC)) = 0 det(XI -(A- BKX)) = 0

(6.23)

As such, one can design the predictive control law and the observer separately.

6.8 Walking on stairs and slope

The AMPC algorithm can also be applied to control a robot walking on a slope, on stairs and clearing an obstacle, as depicted in Figure 22 (a) and (b). The joints' trajectory for straight walking for two steps on flat surface is shown in Figure 23.

(a) Walking on a slope (b) Walking on stairs

Figure 22. Robot walking on different surface

(a) Left leg joints' angle

(a) Right leg joints' angle

Figure 23. The robot joints' trajectories (walking two steps on a flat surface).

6.9 Step over obstacle

Controlling the robot to step over an obstacle without tipping over is quite a challenging task. The general procedure is to first define the constraints on the leg joints limited by its leg structure and kinematics, followed by formulating the balance and collision-free constraints and finally deriving the mathematical models. The stepping over an obstacle trajectory can be achieved by using a setpoint method, a polynomial function or optimal path search algorithms. The motion sequences are depicted in Figure 24. The eight basic step sequences are outline as follows:

Step 1: double support, robot in initial posture, ready to step over obstacle.

Step 2: double support, the robot move its pelvis (CoM) to the supporting leg.

Step 3: single support, the robot lifts its swing leg, moves the

swing foot over the obstacle. Step 4: double support, robot lands its swing leg on the other side of the obstacle.

Step 5: double support, the robot moves it pelvis (CoM) toward

the front support leg. Step 6: single support, the robot lifts its rear leg, swing it

upward and over to clear the obstacle. Step 7: double support, robot moves back to initial posture. Step 8: double support, ready for next action.

In this paper, the sensory feedback system and the control algorithm structure for a bipedal robot TPinokio is presented. A new control algorithm with an AMPC method is also proposed and the simulation results are presented and compared with the preview control method. With a trade off in sampling and update rate, it is possible to implement the control algorithm with on-line updates close to real-time by using a high-speed computer. The current work implements this algorithm with an NI PXI real-time system. Future work will combine the discrete-time AMPC with the Laguerre function and the observer [5] to further enhance and refine the tracking algorithm and the joints' trajectory in order to reduce the effect of the undershoot water-bed phenomenon.

For more details and updates on the development progress, readers may visit the website www.tpinokio.com.

8. References

[1] S. Kajita, F. Kahehiro, K. Kaneko, K. Fujiwara, K.

Harada, K. Yokoi, H. Hirukawa (2003) Biped Walking Pattern Generation using Preview Control of the Zero-Moment-Point, Vol. 2, IEEE International Conf on Robotics and Automation, September 2003, pp. 1620-1626.

[2] M.Vukobratovic, B. Borovac (2004) ZMP - Thirty five

years of its life. International Journal of Humanoid Robotics, 2004, pp. 157-173.

[3] J.B Hoagg, D.S. Bernstein (2007) Nonminimum-Phase

Zeros: Much To Do About Nothing, Part II, Humanoid Robots, IEEE Control System Magazine, Jine 2007, pp. 45-57.

[4] T.C. Wee, A. Astolfi, Xie (2011) 'Modular-Joint Design

of a TPINOKIO Bipedal Robot', ICINCO 2011. pp. 185-190.

[5] A. Astolfi, R. Ortega, A. Venkatraman (2010) Global

observer design for mechanical systems with non-holonomic constraints, American Control Conference (ACC), 2010, pp. 202-207.

[6] M. Xie, G.Q. Zhang, H. Ying (2009) Planning and

Control of Biped Walking along Curved Paths on Unknown and Uneven Terrain. ICIRA 2009, Springer-Verlag, 2009, pp. 1032-1043.

[7] R.N. Jazar (2010) Theory of Applied Robotics,

Kinematics, Dynamics, and Control. Book Springer 2010.

[8] E.R. Westervelt, J.W. Grizzle, C. Chevallereau, J. H.

Choi (2007) Feedback Control of Dynamic Bipedal Robot Locomotion, Book, CRC Press 2007.

[9] S.Kajita, F.Kanehiro, K.Kaneko, K.Fujiwara, K.Harada,

K.Yokoi, H.Hirukawa (2003) Resolved Momentum Control: Humanoid Motion Planning based on the Linear and Angular Momentum, Proceedings of the 2003 IEEE/RSJ Intl. Conference on Intelligent Robots and Systems, October 2003, pp. 1644-1650.

[10] L. Wang (2009) Model Predictive Control System Design and Implementation using MATLAB, Book, Springer 2009.

[11] J.W. Grizzle, C. Chevallereau, A.D. Ames, R.W. Sinnet (2010) 3D Bipedal Robotic Walking: Models, Feedback Control, and Open Problems, NOLCOS, Bologna, Italy, September 2010.

[12] T.C. Wee, A. Astolfi, M. Xie (2011) Modular-Joint Design for Efficient Walking and Modeling of a Bipedal Robot TPinokio, CLAWAR, 2011.

[13] M. Xie, Fundamentals of robotics: linking perception to action, Book, World Scientific Publishing, 2003.

[14] V. Duindam, S. Stramigioli, Modeling and Control for Efficient Bipedal Walking Robots: A Port-Based Approach, Book, Springer 2009.

[15] T. C. Wee, A. Astolfi, M. Xie (2012), Robust Control of Bipedal Humanoid (TPinokio), Journal Procedia Engineering Vol. 41, Elsevier, pp. 643 - 649.

Figure 24. Step over obstacle sequences

7. Conclusion