Active balance of humanoid movement based on dynamic task-prior system

Transcription

1 Research Article Active balance of humanoid movement based on dynamic task-prior system International Journal of Advanced Robotic Systems May-June 2017: 1 13 ª The Author(s) 2017 DOI: / journals.sagepub.com/home/arx Chengju Liu, Jing Ning, Kang An and Qijun Chen Abstract To maintain human-like active balance in a humanoid robot, this article proposes a dynamic priority-based multitask algorithm to avoid self-collision during highly complex robotic whole-body motions and rebalance after external disturbance using momentum compensation strategies. On the one hand, the conflict between self-collision and self-balance constraints in task-space merging with end-effector tracking tasks are considered in the multitask algorithm to improve the robot s balance. On the other hand, for self-balance during the robot s movement, momentum compensation is considered as one task and utilized to reject unknown disturbances. Two strategies are put forward to restrain the sudden change in momentum. One is to calculate the correction in the joint space with the resolved momentum control (RMC) method; the other is to add an end-effector tracking motion in the task space. Simulations and experiments on a full-body humanoid robot validate the effectiveness of the proposed method. Experiments on both a simulation and a full-body humanoid are designed to validate the task-prior algorithm. With the proposed method, the humanoid robot succeeds in avoiding self-collision during movement and is able to rectify itself to the preplanned steady stance while encountering undefined external forces. Keywords Humanoid walking, active balance, multitask system, self-collision avoidance, momentum compensation Date received: 28 November 2016; accepted: 4 April 2017 Topic: Special Issue - Multi-Modal Fusion for Robotics Topic Editor: Huaping Liu Associate Editor: Huaping Liu Introduction Robust locomotion is fundamental for a humanoid robot to maneuver in an unstructured and undetermined human environment. To realize the biped robust walking, multimodal data fusion technologies, 1 7 the trajectory online generation methods, 8 10 and the online optimization strategies are all needed to be integrated to realize maximum degree of biped walking robustness. Currently, humanoid walking is commonly realized by planning the center of mass (CoM) trajectories so that the resultant zero moment point (ZMP) 11,12 trajectory follows a desired ZMP trajectory. For real-time implementation, a humanoid robot was represented by a mass-concentrated model, whereas a simplified dynamics model is generally used. 13,14 However, while these control methods can realize humanoids walking in an ideal environment, small irregularities of terrain or a change in the mass distribution of the CoM may cause the robots to lose balance during walking. To realize adaptive walking in unknown conditions, disturbances from the external environment, such as pushes and terrain irregularities, and self-collision with the robot itself need to be considered. The state of the art balance control of biped robots has solved different problems of movement tasks. However, School of Electronics and Information Engineering, Tongji University, Shanghai, China Corresponding author: Chengju Liu, School of Electronics and Information Engineering, Tongji University, Room 719A, Zhixin Building, Tongji University, Cao an Road 4800#, Shanghai , China. liuchengju@tongji.edu.cn Creative Commons CC BY: This article is distributed under the terms of the Creative Commons Attribution 4.0 License ( which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages ( open-access-at-sage).

2 2 International Journal of Advanced Robotic Systems there is no complete control framework that embraces different conditions, including stand balancing, walking balance with disturbance, self-collision avoidance, and reactive stepping from standing position. In task-space-based robot motion control, the priority of the subtasks is often fixed. However, when the number of subtasks in the robot motion control is large and the coupling of each subtask is complicated, this simple static priority strategy can no longer be applied to the cooperation among the control tasks. For example, on the one hand, in humanoid robot walking control, the walking foot of the robot needs to complete the tracking task of the scheduled walking track, and on the other hand, it can be used to generate the momentum compensation movement needed to improve the balance of the humanoid robot while walking. However, a higher priority of the trajectory tracking task does not always exist. When the robot is in the bipedal support stage, it is of course necessary for the footrest to satisfy the constraint requirement that the relative support foot has no movement and is in close contact with the ground. During the robot support stage, the control of the robot s foot is only concerned with its position and position tracking during the rising and falling phases and does not care about the movement of the foot in the swing phase. At this time, the swing foot can be used to complete the more important task of momentum compensation. Therefore, a more reasonable strategy in robot motion control is that the control subtasks can dynamically switch priorities according to the current control needs during the movement of the robot and realize more flexible multitask cooperative control. Therefore, presenting a frame of the robot motion task space control method with dynamic priority switching is very necessary. In the dynamic priority robotic task space control framework, the system first determines the priorities of each subtask dynamically according to the current requirements of robot motion control and ranks the tasks according to the priority order. The underlying task first maps the motion control output to the zero space of the upper layer task control according to the differential motion control and the corresponding Jacobian matrix and then merges the mapped control output into the upper-layer task control output. The process is iterated step by step and, with the additional constraints (such as robot joint clipping, joint movement speed and collision constraints and other constraints), eventually produces the joint motion of the robot command to complete the robot s integrated motion control. In this article, a novel online active balance control framework based on a dynamic priority-based multitask algorithm is presented. The current balance strategies can be treated as subtasks in the dynamic multitask algorithm, whereas the self-collision avoidance, foot position compensation, and momentum compensation strategies are treated as tasks in the dynamic priority-based multitask algorithm. In this work, we pay attention to the self-collision avoidance and momentum compensation strategies during standing and walking balance with external disturbances. For self-avoidance in humanoid robots during whole-body movement, such as swinging arms, three steps are applied to address the problem: first, to construct bounding segments of the robot according to 3-D geometry; second, to select candidate pairs through prior knowledge, and finally, to achieve the task of avoiding self-collision. For the momentum compensation strategies, from the perspective of momentum, the robot falling down equals a dramatic increase in angular momentum if one of the feet is regarded as a fulcrum. The humanoid s base angle is utilized as the feedback signal to compensate for angular momentum. Meanwhile, if the disturbance causes a deviation of the CoM, linear momentum needs to be compensated as well. The resolved motion rate control (RMRC) method/differential inverse kinematics (DIK) is introduced to solve the redundant problem. 15 The null-space method is utilized to address task priority. 16 The influences of external perturbations on robots can be regarded as the offset of CoM and the change in momentum. The prior-based dynamic multitasking system is used to switch and merge the trajectory tracking task and the momentum compensation task. This article is organized as follows: The related work about how to improve biped walking stability and maintain balance is introduced in the second section. Third section elaborates on the basic theory, including redundant kinematics and the multitask mechanism; gait generation based on linear inverted pendulum mode (LIPM); and the resolved momentum control method. Self-collision constraints are explained in the fourth section. The presented momentum compensation strategy is introduced in the fifth section. Sixth section verifies the validity of the multitask mechanism with self-collision avoidance and momentum compensation algorithms by simulation experiments. We conclude this study and suggest future work in the seventh section. Related work Self-collision is an important physical constraint on wholebody motion planning for a humanoid robot. During task operating, the swing arm may collide with the leg during biped walking, which will affect the balance of walking. Therefore, self-collision is an unavoidable problem in humanoid robot balancing and needs to be considered during adaptive walking. Kwon et al. 17 extended the research area from traditional collision between the robot and the external environment to self-collision with the robot itself. If several tasks are to be executed at the same time, task space methods can be applied to the multitask problem under redundant joint space, which has a low time cost and divides complicated tasks into uncomplicated tasks to represent a complex joint space. Nakanishi et al. 18 implemented multiple tasks under a task space by priority allocation. Sugiura et al. 19 proposed that self-collision can be described as a task in a task space and gives the function

3 Liu et al. 3 of collision avoidance, but this was based on partial coordinates. Cheng et al. 20 invented the concept of self-collision avoidance, with a focus of interest on dynamically predicting and selecting link pairs to be checked. To improve biped walking stability and maintain balance, some strategies as to how to effectively use the whole body to improve walking ability have been proposed For example, arm-swing strategies 25,26 are used during biped walking to compensate for the movements of the swing leg and stabilize robot posture. Currently, although several types of arm-swing strategies have been proposed, they are difficult to execute simultaneously. Kobayashi et al. 27 proposed a two-stage integration method of the two arm-swing strategies for bipedal walking to enhance both stability and efficiency. To achieve external environment disturbance rebalancing, different strategies have been proposed to compensate for nonzero variations in momentum, 22,28,29 regulate CoM, or online adjust the foot position through sensory feedback control Considering humanoid momentum compensation, Englsberger and Ott 28 invented a walking methodology based on a capture point, which integrates CoM vertical motion and angular momentum change. Ugurlu et al. 22 created a strategy to rotate a humanoid s upper body to compensate for undesired yaw movement when the robot walks. Chang et al. 29 estimated the robot state and regulated the angular momentum of CoM and steps to recover from a push, but pushes from left or right side were not examined. Dynamically adjustable foot positioning is an effective strategy for active balance. In our previous work, 30,31 a foot-positioning compensator was proposed to dynamically adjust the foot positioning of a humanoid robot according to the real-time state of the robot s CoM. Castano et al. 32 proposed a new online walking control that replanned the gait pattern based on foot placement control using the actual CoM state feedback. The analytical solution of foot placement is formulated based on the LIPM to recover the walking velocity and reject external disturbances. For the disturbance rejection of continuous walking, Wieber et al. 33,34 proposed an online walking gait generation approach, with both adjustable foot positioning and step duration based on the linear model predictive control scheme. However, only simulated experiments were developed, and no supporting area constraint was considered. Based on the preview control scheme, Nishiwaki and Kagami 35 proposed strategies to adjust future foot positioning and ZMP trajectory. Several typical experiments were developed on a real full-body humanoid robot to validate the effectiveness of their methods. Similar works also included foot positioning adjustment based on a feasible region and avoidance behavior from external forces for a human-carrying biped. 36,37 Stephens 38 utilized an integral control method to maintain standing balance and viewed the disturbance as impulsive. Liu and Atkeson 39 studied the balance of a robot standing based on a trajectory library, which is time consuming. These researchers only considered a robot s balancing under a static state rather than a dynamic steady state. Graf and Röfer 40 presented a robust closed-loop gait, which allowed the robot to react to external perturbations. Urbann and Hofmann 41 presented a reactive stepping algorithm based on the generation of walking motions with sensor feedback. The closed-form calculation of foot placement modifications, such as disturbances of the CoM position, could be balanced with negligible ZMP deviations. As shown in the aforementioned related works, the stateof-the-art balance control of biped robots has solved different problems of the movement tasks. However, there is no complete control framework that embraces different conditions, including stand balancing, walking balance with disturbance, self-collision avoidance, and reactive stepping from standing position. Algorithms A dynamic priority-based multitask mechanism and the related algorithms, including gait generation based on LIPM and the resolved momentum control method, are introduced in this section. Multitasks are merged in the way of a null-space projection according to their priorities. Self-collision avoidance and momentum compensation are considered as tasks and utilized to reject robot itself and external disturbances. Redundant kinematics and multitask mechanism The relationship between task space and joint space in the robot system can be described by a nonlinear function as follows T ¼ f ðyþ (1) where T ¼½T 1 T M Š T represents the manipulator endeffector s position and rotation, and y i in y ¼½y 1 ; ; y N Š T represents the link i s rotation or displacement relative to link i 1. The RMRC method is adopted to control the velocities of joints instead of directly controlling the joints. The differential kinematics and inverse kinematics of the robot system can be expressed as follows T_ ¼ JðyÞ y _ (2) _y ¼ J 1 ðyþ _ T (3) where JðyÞ is the Jacobian matrix and J 1 ðyþ is the inverse Jacobian matrix, which exists when JðyÞ is full rank. If N > M in equation (1), the robot system is redundant. Using a pseudo-inverse matrix instead of the inverse Jacobian matrix provides the following J # ¼ J T ðjj T Þ 1 (4)

4 4 International Journal of Advanced Robotic Systems r y x Q s r Figure 2. xy-cross section showing the step size s and the inverted pendulum origins r and r. Figure 1. Priority-based dynamic multitask frame. To avoid the singular problem, the damped least squares (DLS)methodcanbeappliedtoconstructthepseudoinverse matrix J # ¼ðJ T J þ l 2 IÞ 1 J T (5) where l is the damped constant value, which will ensure Jacobian matrix J # is full rank. Null space is defined as the linear combination of certain joints that will not make a difference in the motion of the end-effector, which can be expressed as N ¼ I J # J (6) With the help of a null-space projection operator, multitasks with different priorities can be executed at the same time, which can be described as follows # _y ¼ J 1 T_ # 1 þ N 1 J 2 T_ 2 (7) where task T 1 is prior to task T 2, and T 2 has no influence on T 1 through a null-space projection. The block diagram of the dynamic multitask mechanism based on priority is shown in Figure 1, where the higher priority task will be projected onto the null space of its lower priority task; finally, by linear superposition and an inverse kinematics calculation, the robot joint space can be configured. Gait generation based on LIPM In this section, a 3-D-LIPM is used to plan the CoM motion of the robot in order to generate a stable periodic gait. Then, the support foot and swing foot trajectories are designed according to the planned CoM. 40 The position and velocity of the CoM, relative to the origin of the inverted pendulum, are given by 1 xðtþ ¼x 0 coshðktþþ _x 0 sinhðktþ (8) k _xðtþ ¼x 0 k sinhðktþþ _x 0 coshðktþ (9) p where k ¼ ffiffiffiffiffiffiffi g=h ; g is the gravitational acceleration; h is the height of the CoM above the ground; and x 0 and _x 0 are the position and velocity of the CoM relative to the origin of the inverted pendulum at t ¼ 0, respectively. To eliminate a double-support phase, the point in time for altering the support leg must be determined. First, the point in time t ¼ 0 is defined as the time point for altering the support leg. When t ¼ 0, the y-component of the velocity is 0(ð _x 0 Þ y ¼ 0). The position of the CoM at this point,ðx 0 Þ y, is an arbitrary parameter and has a value greater or less than 0, depending on the active support leg. Thus, the function of the CoM s position in the y-direction during the range of t ¼ t b and t t e is as follows, which means a single-support phase starts at t ¼ t b and ends at t ¼ t e x y ðtþ ¼ðx 0 Þ y coshðktþ (10) If the swinging foot should be placed with a distance of r y þ s y r y relative to the supporting foot (Figure 2) at the end of the single-support phase, and if xðtþ and _xðtþ are the position and velocity of the CoM relative to the next pendulum origin, the point in time to alter the support leg can be determined by finding the ending of a single-support phase t e where ðxðt e ÞÞ y ðxðt b ÞÞ y ¼ r y þ s y r y (11) ð _xðt e ÞÞ y ¼ð_xðt b ÞÞ y (12) where t e and t b are calculated with an iterative method, 40 in which t e is initially guessed. Equation (10) can be transformed into t b ¼ 1 k arcsin h ðx! 0Þ y k sinhðkt e Þ ðx 0 Þ (13) yk to compute t b that matches t e. The guessed t e can then be determined using the velocity of the CoM at t e and length ðxðt e ÞÞ y ðxðt b ÞÞ y. To cover a step size, s x, the origin of the inverted pendulum of the next single-support phase should be placed at

5 Liu et al. 5 adistanceofr x þ s x r x from the origin of the current inverted pendulum. Additionally, the velocity in the x direction should remain the same value when the support leg alternates. Thus, the following equations can be obtained ðxðt e ÞÞ x ðxðt b ÞÞ x ¼ r x þ s x r x (14) ð _xðt e ÞÞ x ¼ð_xðt b ÞÞ x (15) To make sure that the position of the origin of the inverse pendulum is optimal in the next phase, we designate that r x ¼ 0 and ðxð0þþ x ¼ 0. When the CoM has the position x tb and velocity _x tb relative to the origin of Q (Q is located between the two feet as shown in Figure 2) at the beginning of a single-support phase, r x, ðx 0 Þ x and ð _x 0 Þ x can be computed using the following linear system of equations so that a complete set of pendulum parameters can be determined sinhðkt b Þ r x þðx 0 Þ x coshðkt b Þþð_x 0 Þ x ¼ðx tb Þ k x ðx 0 Þ x k sinhðkt b Þþð_x 0 Þ x coshðkt b Þ¼ð_x tb Þ x ðx 0 Þ x k sinhðkt e Þþð_x 0 Þ x coshðkt e Þ ðx 0 Þ x k sinhðkt b Þ ð_x 0 Þ x coshðkt b Þ¼0 sinhðkt e Þ r x þðx 0 Þ x coshðkt e Þþð_x 0 Þ x ðx 0 Þ k x coshðkt b Þ ð_x 0 Þ x sinhð kt b Þ k ¼ r x þ s x (16) After the CoM trajectory is known, the position of the support foot is given by the functions of x x ðtþ and x y ðtþ. The swing foot s trajectory, however, must be designed. The known position and length of the current singlesupport phase is used to obtain a percentage of the progressing phase as follows ðtþ ¼ t t b (17) t e t b The swing foot should be repositioned to the origin of the next inverse pendulum during the single-support phase. The motion of the swing foot contains movement in the xydirection and lift in the z-direction, which are defined relative to Q as follows mðtþ ¼ðs xy þ s xy Þ 1 cosð ðtþpþ s xy þf y (18) 2 1 cosð ðtþ2pþ lðtþ ¼s z (19) 2 where s xy and s xy are the current and next step size in the xy-direction, respectively;s z is the lift height in the z-direction; and f y is the foot position which only contains a y-component. Thus, the trajectory of the swinging foot can be determined. Finally, combining the target foot positions relative to the CoM and the CoM position relative to the torso by computing the current robot s stance, joint angles can be solved through inverse kinematics. Resolved momentum control The linear momentum, P ¼½P x P y P z Š T, and angular momentum, L ¼½L x L y L z Š T, refer to a robot CoM s changing rate, which are related to the stability and balance of the robot. For the whole system, momentum can be calculated as P ¼ ~me ~m^r " # B!~c M _y X_ B L 0 I H _y _y (20) where ~m is the mass of the whole robot; E is the 3 3 identity matrix; r B!~c is a 3 1 vector from base to CoM; ~I is the inertial matrix with respect to CoM; X_ v T B w T B ŠT is marked as the velocity of the robot s base; and^is an equivalent operator to a cross product that translates a 3 1 vector into a 3 3 matrix. M y _ and H_ y are 3 n matrices that represent the relationship between momentum and joint velocity, respectively. If the robot is modeled as several cascade links connected to the base link, M y _ and H_ y can be calculated in a recursive method. If we divide y, _ M y _, and H_ y into left leg joints, right leg joints, and passive joints, respectively, that means h i T _q ¼ _q T leg 1 _q T _q T leg 2 passive (21) M y _ ¼ ½ M leg 1 M leg1 M passive Š (22) H y _ ¼ ½ H leg 1 H leg1 H passive Š (23) where i ¼ 1 represents the left leg and i ¼ 2 represents the right leg. Then, equation (20) can be rewritten as follows P ¼ ~me ~m^r B!~c X_ B þ X2 M legi _y legi L 0 I H i¼1 legi þ M passive _y passive (24) H passive Considering that the humanoid s base is moving, the coordinate should be placed at a point free from the robot. If we choose the center between the two feet, which is fixed on the ground, as the coordinate origin and call this coordinate S F, the computation of a foot s velocity has to be corrected as follows X_ Fi ¼ E ^r B!F i X_ B þ J legi ylegi _ (25) 0 E where X v T F i w T F i Š T is marked as the velocity vector of the foot and r B!Fi is a 3 1 vector from base to the ith foot.

6 6 International Journal of Advanced Robotic Systems Figure 4. 3-D model represented by OBBs and distance between segment pair. OBB: object bounding boxe. Figure 3. Main flow from building 3-D model to collision avoidance. Then, the joint velocity of each leg can be computed as follows _y legi ¼ J 1 X_ leg Fi J 1 E ^r B!Fi X_ i leg B (26) i 0 E If equation (26) is substituted into equation (24), the momentum formula can be rewritten as follows " P ¼ M #" # " B M passive X_ B L HB þ X2 MF # i X_ Fi H passive where M B H B _y passive i¼1 H F i = ~me ~m^r " B!~c X2 M # F i E ^rb!fi 0 I i¼1 0 E " # M F i H F i = M leg i H legi H F i (27) (28) J 1 leg (29) With the equations above, if the reference momentum, ½P ref ; L ref Š T, and the two feet s reference velocity relative to base X_ ref F i are given, the base velocity, X_ B, and joint velocity, y, _ of the robot can be computed. Self-collision constraints When considering robot modeling, most researches simplify the robot to skeleton, but self-collision problem normally happens because of the 3-D geometry of robot. Self-collision means part of the robot may collide with other part of the robot. Collision-free path planning can be treated as an important constraint or task in robot motion planning and a necessary technique to maintain balance. To achieve this constraint, self-collision should first be detected. The whole architecture of the self-collision task is shown in Figure 3. The first step constructs bounding segments of the robot according to 3-D geometry, then selects candidate pairs through prior knowledge, and finally achieves the task of self-collision avoidance. The state postures of the links are related to the transform matrix, which varies along with the configuration of the joint space state. We use object bounding boxes (OBBs) to describe the robot s three-dimensional model (Figure 4) according to the joint connections of the robot. For a humanoid robot, the three-dimensional model of the robot can be divided into N segments. If s i represents one segment, the whole three-dimensional model can be expressed as S ¼fsigi ¼ 0::N and M j i ¼ Pair i6¼j fs i ; s j g, any two segments from these N segments are defined as a segment pair. For the collision checking of the simple OBB geometry, the separate axis theorem (SAT) is applied for fast collision detection between polygon meshes. 42 Differential distance calculation, by obtaining the closest point pair (Figure 4), can be expressed as d_ i;j ¼ f _ ðyþ y _ ¼ J i;j _ coll y (30) Collision avoidance can be considered as a subtask, T coll, in the task space, which aims to enlarge the distance, d i;j, between segment pair. Thus, the joint space configuration can be rewritten as # _y ¼ J coll T_ # coll þ N coll J 2 T_ 2 (31) where T coll is the collision avoidance task, which has a higher priority than T 2. Momentum compensation strategy Compensation setting It is known that the process of a robot falling down when it encounters disturbances can be considered as a mutation of the trunk angle under S F. From the perspective of momentum, a robot falling down equals a dramatic increase in angular momentum if one of the feet is regarded as a fulcrum. Therefore, we need to detect the humanoid s base angle, y B, with the help of the embedded inertial unit

7 Liu et al. 7 Figure 5. Humanoid robot platform. (a) NAO robot. (b) Approximate dynamic representation of each link of the robot. (c) The simulation environment (Webots). sensor. Then, y B is utilized as the feedback signal to compensate for angular momentum. Meanwhile, if a disturbance causes the deviation of the CoM, linear momentum needs to be compensated as well. When the momentum is compensated with the proportional control method, the formula can be expressed as DP ¼ K p1 ðp ref CoM p CoMÞ ~m (32) DL ¼ K p2 ðy ref B y BÞ~I (33) where K p1 and K p2 are scaling factors of linear momentum and angular, respectively; p ref and p CoM CoM refer to the expected CoM and measured CoM, respectively; y ref B and y B represent the expected trunk angle and measured trunk angle, respectively. Under general circumstances, we assume that y ref B ¼½0 0 0ŠT, which means that the robot s base is upright to the ground. In the RMC method, we set _ F i ¼ 0, denoting that the generated rate vector of joints is only used for momentum compensation. After the base rate, X_ B, and joint rate, y _ MLegi, for both legs are computed with the RMC formulas, the rotation of base and leg joints under the current joint configuration are computed as X ref Dynamic multitask integration y B ¼ y B þ o B (34) y Legi ¼ y Legi þ _ y MLegi (35) If the foot i is executing a trajectory tracking task T tracki,at the same time, the momentum compensation task should be considered as an extra task and be merged to T tracki, which can be written as follows _y Legi ¼ J Fi # _ T tracki þ N Fi _ ymlegi (36) When a robot encounters a strong disturbance, it may fail to adjust its stance promptly and avoid falling because of its intrinsic constraints, such as the limit of joint velocity. Therefore, we consider adding an extra momentum compensation task in the task space that is obtained by the RMRC method but not solved in the RMC manner. Compared with human actions, when an external force is received, it is found that a foot lift task opposite to the body tilt direction can be regarded as an effective solution to resisting an angular momentum change. In this way, when a robot s body has sufficient tilt, a foot lift task is added to maintain balance. The foot lift task can be merged with the momentum compensation task as follows _y Legi ¼ J Fi # _ T lifti þ N Fi _ ymlegi (37) where T lifti represents the lift task of foot i. Simulation and experiment Experiment platform To validate the proposed method, the robot NAO is used for the experiments (Figure 5(a)). The robot has 25 degrees of freedom (DOF), with 11 DOFs for the two legs. The height of this robot is approximately 50 cm during walking and its weight is 4.35 kg. The sensor data and the walking control module update at a rate of 50 frames per second. The difficulty in developing walking approaches on this robot lies in that each leg weighs almost the same as its body and the links of the robot are made of plastic, which makes the dynamics of the robot quite different from the simplified LIPM model. Additionally, disturbances brought with the deformation of the plastic links are also significant. To compute the local inertial tensor of each link of the body, the geometrical approximation of each part of the robot is given by the inertia tensor formula. As shown in Figure 5(b), the head of the robot is represented by a sphere of uniform quality, and the other parts are represented by rectangular parallelepipeds with uniform mass. The simulations are carried out under a physical simulation environment (Webots), as shown in Figure 5(c), and then dubbed to the real robot.

8 8 International Journal of Advanced Robotic Systems Figure 6. Trajectory of right hand with and without collision avoidance. Figure 8. Vertical collision avoidance snapshots. Collides with (a) and successful collision avoidance (b). Figure 7. Distance between and before and after works. Self-collision avoidance In this section, we combine the arm swing task with a selfcollision avoidance task. In the first experiment, the arm is designed to swing vertically with a shoulder pitch angle varyingintherangeof½30 ; 110 Š, which fulfills the requirement of fetching objects or swinging during standing and walking. Under this circumstance, the right hand s trajectory is computed through cubic polynomial interpolation. We choose the segments of lower arm and upper leg as the potential collision pair and detected the distance, d, between them in real time. If d is smaller than a specific value (such as 0.03 m), the self-collision avoiding task, T col, is added to the end-effector tracking task. Figure 5 demonstrates the trajectory of the right hand in the x, y, and z directions before and after collision avoidance. The distance between the right lower arm, S RLA, and right upper leg, S RUL, is shown in Figure 6, where d is enlarged by T col. The serrated waves shown in Figures 6 and 7 are due to the existence of T col. Simulations of the robot arm swing with and without collision avoidance are depicted in Figure 8. Figure 9. Snapshots of the collision avoidance during biped walking. (a) Walking with arm swing. (b) Bending forward. (c) Horizontal collision avoidance during arm swing. Furthermore, if we add arm-swing motions during biped walking, collision avoidance should be considered in order to maintain balance. Assuming that the walking task has a higher priority than the arm-swinging task. The robot can walk forward on a flat terrain stably while swinging both arms forward and backward, alternatively, to avoid self-collision. As shown in Figure 9, the second experiment is set as including walking, bending forward, and arm swinging, which are then converted to motion planning tasks merged with self-balancing constraints. When bending forward, the robot is expected to maintain balance at the same time. We design the CoM to move

9 Liu et al Roll Pitch Heading Body angle (rad) Time (s) Figure 10. Body angle curve. forward and downward slightly with momentum control method. Also, we set robot s feet standing on the ground and detect whether the CoM leaves the supporting polygon. Momentum compensation while standing Momentum compensation is applied under two circumstances in this experiment. One circumstance applies a sudden force to the robot when it is in a steady state, such as standing on the ground with double feet. The other creates disturbances while the robot is walking with a predefined fixed gait. The robot is in a static state while standing on the ground, which means the reference momentum is zero. If a sudden force is applied to the robot, its trunk will lean to one side and a CoM error will be caused. If the standing state is to be maintained, p ref CoM in equation (30) and yref B in equation (31) should be set to the CoM and body angle in the initial standing state, respectively. Dynamic multitasking should be assigned as follows ( J _y # _ F Leg1 ¼ 1 T lift1 þ N F1 ymleg1 _ y > y ref Bx Bx (38) J F # _ 1 T stand1 þ N F1 ymleg1 _ else ( J _y # _ F Leg2 ¼ 2 T lift2 þ N F2 ymleg2 _ y < y ref Bx Bx (39) J F # _ 2 T stand2 þ N F2 ymleg2 _ else where T stand1 and T stand2 represent the standing tasks of left foot and right foot, respectively, which means the foot is static and has parallel contact with the ground; T lift1 and T lift2 represent lift motion in the y z plane; and y ref B x is a positive threshold value which determines when multitasks are switched. Under such dynamic multitask strategy, the robot will remain standing on the ground and perform the momentum compensation if it just slants slightly but will lift the opposite foot in the tilt s direction and compensate momentum if the tilt exceeds the threshold. The body angle curve, the expected CoM versus measured CoM trajectories, and double feet trajectories are shown in Figures 10, 11, and 12, x (m) y (m) z (m) 8 Expected CoM Measured CoM Time (s) Time (s) Time (s) Figure 11. Trajectories of the expected CoM and measured CoM. CoM: center of mass. x (m) y (m) z (m) Time (s) Time (s) Left foot respectively (relative to S F ). The experimental results show that the robot inclines to the left and lifts the right foot because of the applied force occurring at approximately t ¼ 3s and then inclines to right, lifting the left foot and swaying backward and forward slightly due to the counterforce of the ground while putting down the right foot. At last, the robot returns to an upright state due to momentum compensation. Figure 13 demonstrates the snapshot sequences of the robot recovering from an external lateral disturbances, the largest force in this lateral disturbance rejection experiment almost with a maximum value of 10.8 N (this is just the maximum value in this simulation and not the maximum value for the NAO robot). Momentum compensation during walking Right foot Time (s) Figure 12. Trajectories of left and right feet. In this section, the 3-D-LIPM is used to plan the CoM motion to generate a stable periodic gait; then, the support

10 10 International Journal of Advanced Robotic Systems Figure 14. Sequences of robust walking. (a) Lateral disturbance from left hand. (b) Lateral disturbance from right hand. Figure 13. Snapshots sequences of NAO recovering from lateral external disturbance. (a) Small lateral disturbance from left hand. (b) Small lateral disturbance from right hand. (c) Larger lateral disturbance from left hand. (d) Larger lateral disturbance from right hand. foot and swing foot trajectories are designed according to the CoM. Uncertain disturbances are applied to the robot during this dynamic steady process. The term p ref CoM,in equation (32), is the output of LIPM, and y ref B in equation (33) is set to zero. In this case, the left foot and right foot alternate to function as the swing foot and support foot, and dynamic multitask integration is designed as follows 8 >< _y swleg ¼ >: 8 >< _y spleg ¼ >: J swf # T _ swflift þ N swf J swf # T _ swf J swf # T _ ; swf þ N swf ymswleg _ J # spf _ T spf J # spf _ T spf þ N spf _ ymspleg ; y Bx y Bx > y ref Bx p swf z > h F 5 else (40) > y ref Bx p swf z > h F 5 else (41) where T swf and T spf are the foot tracking trajectories of the walking task; T swflift is the lift motion in the y z plane added to the swing foot; p swfz is the height of the swing foot above ground; and h F is the maximum lift height of the swing foot motion while walking. As is expressed in the equations above, a foot lift task will be added to the swing foot, which has a higher priority than its original swing task if the robot s tilt is large Figure 15. Body attitude curve during walking. enough. Meanwhile, _ y MspLeg and _ y MswLeg are not included in momentum compensation because the foot lift task is actually designed for rejecting a sudden change in momentum. In other cases, the walking tasks and the momentum compensation task are executed at the same time. The task switching condition requires the swing foot to exceed a specific height to avoid friction between the foot and the ground. Additionally, it is worth noting that the foot lift task will not be active if the robot inclines in the opposite direction of the support foot (e.g., the robot inclines to the right with left foot support), because a self-collision may occur. The robustness of the reactive gait pattern against the lateral impulse is first studied. Figure 14 shows the sequences of robust walking when the robot has undergone two external forces from different directions. The disturbances appear at about t ¼ 6 s and t ¼ 14 s, both with a maximum value of approximately 9 N and the duration of the disturbance about 0.5 s. The body angle curve, expected CoM versus measured CoM trajectories, and double feet trajectories while walking are shown in Figures 15, 16, and 17, respectively (relative to S F ). If the left-oriented disturbance is imposed on the robot, the robot inclines to the left. When the robot inclines a

11 Liu et al. 11 Figure 16. Trajectories of the expected CoM and measured CoM. CoM: center of mass. Figure 17. Trajectories of the left and right feet. little, the RMC computed joint correction works and the walking task will not be terminated. With the growth of trunk tilt, the right foot lifts up and stretches out until the current single-support phase ends. In the next singlesupport phase, the right foot switches to the support foot and returns to its original position on the ground, while theleftfootpreparestoexecute the swing foot task. The robot moves on to a normal walking state in approximately 3 s. When the right-oriented disturbance occurs, the right foot acts as the support foot, while the left foot plays the role of the swing foot and the robot recovers from the force attack exactly as it did in the left-oriented force disturbance restoration. In addition, the swing foot lift task may cause a tracking error of the CoM, swing foot task, and support foot task but is of great help in maintaining balance. Conclusion and discussion Walking is the basic skill of a humanoid robot. Most of the current walking control methods adopt predefined foot and center of mass trajectories and control the robot with simplified dynamics model. These methods usually cannot resist with external disturbance and lack adaptation to the environment changes and therefore can be hardly used in real on-field applications. In general, the current disturbance rejection methods are mainly divided into balance method based on local acceleration of the body and balance method based on modifying the supporting structure of the robot and environment. These methods can only be effective for small disturbances, or only in the simulation environment. On the other hand, most of the current approaches only use one balance skill, and the skill designed according to anticipate specific behavior. For humanoid, the complex and flexible balance ability is more dependent on a variety of balancing skills integrated application. In this article, a priority-based dynamic multitasking frame is introduced under which multitasks are switched, merged, and executed. Collision avoidance can be considered as a subtask during highly complex robotic wholebody motions in the task space in order to avoid its influence on balance. A momentum compensation method was proposed, which explains that momentum changes caused by disturbances are suppressed by either joint correction computed with the RMC method or by a foot lift motion opposite to the robot s tilt. The validity of the proposed method is verified through experiments on an NAO robot. The results illustrate that, with the introduced algorithm, the robot will adjust itself to the predesigned static or dynamic steady state if uncertain forces are applied. In the next step work, a preview controller can be utilized to estimate the robot state, and more effective momentum compensation tasks, other than foot lift motion, will be added in the dynamic multitasking frame to realize an improved ability of the disturbance rejection. As we know, animals and humans in nature show the strange survival skills not only rely on long-term evolutionary process and genetic congenital acquisition, many behavioral skills are acquired through learning ability acquired. Even some seemingly innate skills are acquired through the practice and improvement can be applied to the current living environment. Such as humanoid balance skills, human needs to attempt many times to get the skills to prevent falling, like skating. Humans can still learn to adapt to walking ability. Another function of acquired learning is to improve the coordination of the various parts of the body and the simultaneous execution of various independent behaviors. As a result of repetitive exercises, we can accomplish tasks such as using the upper body part to complete the side water while balancing the walking. Therefore, to learn to change and improve the skills of humanoid robot is a flexible, robust, efficient, and essential to the ability to walk.

12 12 International Journal of Advanced Robotic Systems In the future work, the mechanism of the balancing skills in human walking will be studied and control methods of using active balancing skills to resist with external disturbance under unstructured and uncertain environment will be explored. Dynamics modeling of local acceleration balancing skills and supporting-polyhedron modification balancing skills will be designed and control architecture of multiple balancing skills cooperation will be investigated. The online skill learning algorithms will be explored in the future research. The work is targeted at exploring new theory and technology of humanoid walking control, breaking through the bottleneck of humanoid robot real application, and solving the basic and key problem of the adaptation of humanoid walking control. The result is critical for the production of new-generation humanoid robot, development of on-field and service robot, assisting robot and space robot, and so on. Acknowledgements The authors would like to express thanks to technicians in the Laboratory of Robotics and Intelligent Systems of Tongji University for their assistance during experiments. Declaration of conflicting interests The author(s) declared no potential conflict of interest with respect to the research, authorship, and/or publication of this article. Funding The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Natural Science Foundation of China (grant nos and ) and the Fundamental Research Funds for the Central Universities, Basic Research Project of Shanghai Science and Technology Commission (grant no. 16JC ). References 1. Lahat D, Adali T and Jutten C. Multimodal data fusion: an overview of methods, challenges, and prospects. Proc IEEE 2015; 103(9): Dobrišek S, Gajšek R, Mihelič F, et al. Towards efficient multi-modal emotion recognition. IntJAdvRoboticSyst 2013; 10(1): Zheng E, Chen B, Wang X, et al. On the design of a wearable multi-sensor system for recognizing motion modes and sit-tostand transition. Int J Adv Robotic Syst 2014; 11(2): Liu H, Sun F, Fang B, et al. Robotic room-level localization using multiple sets of sonar measurements. IEEE Trans Instrum Meas 2017; 66(1): Liu H, Yu Y, Sun F, et al. Visual-tactile fusion for object recognition. IEEE Trans Autom Sci Eng 2017; 14(2): Liu H, Guo D, and Sun F. Object recognition using tactile measurements: Kernel sparse coding methods. IEEE Trans Instrum Meas 2016; 65(3): Liu H, Liu Y, and Sun F. Robust exemplar extraction using structured sparse coding. IEEE Trans Neural Netw Learn Syst 2015; 26(8): Heo JW and Oh JH. Biped walking pattern generation using an analytic method for a unit step with a stationary time interval between steps. IEEE Trans Ind Electron 2015; 62(2): Kim JW. Online joint trajectory generation of human-like biped walking. Int J Adv Robotic Syst 2014; 11(2): Whitman EC, Stephens BJ and Atkeson CG. Torso rotation for push recovery using a simple change of variables. In: IEEE-RAS International Conference on Humanoid Robots, Osaka, Japan, 29 November 1 December 2012, pp IEEE. 11. Huang Q, Yokoi K, Kajita S, et al. Planning walking patterns for a biped robot. IEEE Trans Robotics Autom 2001; 17(3): Seven U, Akbas T, Fidan KC, et al. Bipedal robot walking control on inclined planes by fuzzy reference trajectory modification. Soft Comput 2012; 16(11): Kajita S, Kanehiro F, Kaneko K, et al. The 3D linear inverted pendulum mode: a simple modeling for a biped walking pattern generation. In: IEEE International Conference on Intelligent Robots and Systems (IROS), Maui, HI, 29 October 3 November 2001, pp IEEE. 14. Kajita S, Kanehiro F, Kaneko K, et al. A real time pattern generator for biped walking. In: IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, May 2002, pp IEEE. 15. Umetani Y and Yoshida K. Resolved motion rate control of space manipulators with generalized Jacobian matrix. IEEE Trans Robotics Autom 1989; 5(3): Liegeois A and Liegeois A. Automatic supervisory control of the configuration and behavior of multibody mechanisms. IEEE Trans Syst Man Cyber 1977; 7(12): Kwon S, Chung W, Youm Y, et al. Self-collision avoidance for n-link redundant manipulators. In: IEEE International Conference on System, Man, and Cybernetics, Charlottesville,VA, October 1991, pp IEEE. 18. Nakanishi J, Cory R, Mistry M, et al. Operational space control: a theoretical and empirical comparison. Int J Robotics Res 2008; 27(6): Sugiura H, Gienger M, Janssen H, et al. Real-time selfcollision avoidance for humanoids by means of nullspace criteria and task intervals. In: IEEE-RAS International Conference Humanoid Robots, Genova, Italy, 4 6 December 2006, pp IEEE. 20. Sugiura H, Gienger M, Janssen H, et al. Real-time self-collision avoidance for humanoids by means of nullspace criteria and task intervals. In: IEEE-RAS International Conference Humanoid Robots, Genova, Italy, 4 6 December 2006, pp IEEE. 21. Whitman EC, Stephens BJ, and Atkeson CG. Torso rotation for push recovery using a simple change of variables. In: IEEE-RAS International Conference on Humanoid Robots, Osaka, Japan, 29 November 1 December 2012, pp IEEE.

13 Liu et al Ugurlu B, Saglia JA, Tsagarakis NG, et al. Yaw moment compensation for bipedal robots via intrinsic angular momentum constraint. Int J Humanoid Robotics 2012; 9(04): Luo X, Xia D, and Zhu C. Planning and control of biped robots with upper body. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, South Korea, 9 14 October 2016, pp IEEE. 24. Liu J and Urbann O. Bipedal walking with dynamic balance that involves three-dimensional upper body motion. Robotics Auton Syst 2016; 77: Kuindersma S, Grupen R, and Barto A. Learning dynamic arm motions for postural recovery. In: IEEE-RAS International Conference on Humanoid Robots, Bled, Slovenia, October 2011, pp IEEE. 26. Sobajima M, Kobayashi T, Sekiyama K, et al. Bipedal walking control of humanoid robots by arm-swing. In: SICE Annual Conference, Nagoya Japan, September 2013, pp IEEE. 27. Kobayashi T, Sekiyama K, Aoyama T, et al. Selection of two arm-swing strategies for bipedal walking to enhance both stability and efficiency. Adv Robotics 2016; 30(6): Englsberger J and Ott C. Integration of vertical com motion and angular momentum in an extended capture point tracking controller for bipedal walking. In: IEEE-RAS International Conference on Humanoid Robots, Osaka, Japan, 29 November 1 December 2012, pp IEEE. 29. Chang CH, Huang HP, Hsu HK, et al. Humanoid robot pushrecovery strategy based on CMP criterion and angular momentum regulation. In: IEEE International Conference on Advanced Intelligent Mechatronics (AIM), 2015, Liu CJ, Xu T, Wang DW, et al. Active balance of humanoids with foot positioning compensation and non-parametric adaptation. Robotics Auton Syst 2016; 75: Xu T, Chen Q, and Cai Z. Rebalance strategies for humanoids walking by foot positioning compensator based on adaptive heteroscedastic SpGPs. In: IEEE International Conference on Robotics and Automation (ICRA), 2011, pp Castano JA, Li Z, Zhou C, et al. Dynamic and reactive walking for humanoid robots based on foot placement control. Int J Human Robot 2016; 13(02): Diedam H, Dimitrov D, Wieber PB, et al. Online walking gait generation with adaptive foot positioning through linear model predictive control. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2008, pp Wieber PB. Trajectory free linear model predictive control for stable walking in the presence of strong perturbations. In: IEEE-RAS International Conference on Humanoid Robots, Genova, Italy, 4 6 December 2006, pp IEEE. 35. Nishiwaki K and Kagami S. Strategies for adjusting the ZMP reference trajectory for maintaining balance in humanoid walking. In: IEEE International Conference on Robotics and Automation (ICRA), Anchorage, AK, 3 7 May 2010, pp IEEE. 36. Lee BJ, Stonier D, Kim YD, et al. Modifiable walking pattern of a humanoid robot by using allowable ZMP variation. IEEE Trans on Robot 2008; 24(4): Hashimoto K, Sawato T, Hayashi A, et al. Avoidance behavior from external forces for biped vehicle. In: IEEE International Conference on Robotics and Automation (ICRA), Anchorage, AK, 3 7 May 2010, pp IEEE. 38. Stephens B. Integral control of humanoid balance. In: IEEE/ RSJ International Conference on Intelligent Robots and Systems (IROS), San Diego, CA, 29 October 2 November 2007, pp IEEE. 39. Liu C and Atkeson CG. Standing balance control using a trajectory library. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, October 2009, pp IEEE. 40. Graf C and Röfer T. A closed-loop 3D-LIPM gait for the RoboCup standard platform league humanoid. In: Proceedings of the Fifth Workshop on Humanoid Soccer Robots, 2010, pp Urbann O and Hofmann M. A reactive stepping algorithm based on preview controller with observer for biped robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Korea, 9 14 October 2016, pp Daejeon Convention Center. 42. Gottschalk S. Separating axis theorem. Technical report, Technical Report TR96-024, Department of Computer Science, UNC Chapel Hill, 1996.