IROS2019国际学术会议论文集0950

Nonlinear Optimization of Step Duration and Step Location Jiatao Ding1,2, Xiaohui Xiao1, Nikos Tsagarakis2 Abstract The modulation of step location and duration plays an important role in realizing robust bipedal walking. This paper formulates it as a nonlinear programming problem (NLP) and proposes a novel optimization approach to adjust step location and duration in real time. Based on state feedback, the Linear Inverted Pendulum dynamics is exploited to determine the optimal step parameters. Different from previous works, this work presents three main characteristics: i) the hyperbolic functions of step duration rather than the step duration itself are chosen to be optimization variables; ii) the approach can be switched from baseline two-steps-prediction optimization to one-step-prediction optimization through merely adding several equality constraints in problem formulation; iii) the approach can deal with relative step location tracking (velocity tracking) or absolute step location tracking (position tracking) via changing the reference step parameters. As a result, the fi rst characteristic enables the NLP to be solved in a computational- effi cient manner and the latter two endow the approach with versatility under different control modes. The effectiveness has been demonstrated by simulation experiments. I. INTRODUCTION Stable waking is a prerequisite for humanoid robots that target to enter real application environments. Yet, unstruc- tured environments bring challenges to locomotion control. Among many reactive strategies, step location and duration adjustment plays an essential role in responding timely to external disturbances and uncertainties. Our goal, in this paper, is to propose a fast and effi cient approach with the capability of step location and duration modulation. For step location adjustment, Model Predictive Control (MPC) was proposed in 1, with minimizing the jerk of Center of Mass (CoM). Then, using the Linear Inverted Pendulum Model (LIPM), other MPC strategies have also been developed in 24. Recently, a general nonlinear MPC (NMPC) framework was proposed, which can deal with reactive stepping, vertical CoM motion and angular mo- mentum adaptation simultaneously 5. Besides, non-MPC strategies for reactive stepping were also proposed, such as the works in 6, 7. Nevertheless, the lack of step duration modulation limits the robustness against large disturbances. However, online step duration modulation would cause nonlinearity in locomotion dynamics. To address this prob- lem, a NMPC strategy was proposed in 8, which combined ankle, hip and step strategies. Then, to compute optimal step 1School of Power and Mechanical Engineering, Wuhan University, Wuhan, Hubei Province, P. R. China 430072.jtding, xhxiao. 2Humanoid and Human Centered Mechatronics Research Line, Is- tituto Italiano di Tecnologia, via Morego, 30, Genova, Italy 16163. name.surnameiit.it. durations and locations of multi steps, Nonlinear Program- ming Problems (NLPs) were solved in 9 and 10. Besides, a mixed-integer Quadratic Programming (QP) strategy was also proposed in 11. Yet, above studies were time-cost. To reduce time cost, in 12, an analytic method was proposed to determine the next step time and location, with the aim to recover walking velocity. Using the instantaneous capture point, step duration was solved analytically in 13. Based on the state estimation, another analytic method was proposed in 14, which obtained feasible solutions. To im- prove the disturbance rejection performance, the novel vari- ables representing the exponential function of step duration was introduced in 15. As a result, a QP problem was solved to achieve optimal step parameters of current step. Recently, a fast sequential approach was proposed 16, which obtained sub-optimal solutions of remaining step duration, the next step duration and step location. Apart from the time effi ciency, several essential problems are often neglected in above studies. The fi rst one is, for a given CoM state, how many steps the algorithm should pre- dict so as to achieve ideal disturbance rejection performance. Based on capturability analysis, 17 pointed out that two or three steps look-ahead is suffi cient for most cases. Then, using the LIPM, 18 further argued that two step is enough. As expected, most of above works indeed predicted two or more steps, such as the works in 811. Yet, the work in 15 demonstrated that, if the state of the robot is viable after the push (based on LIPM dynamics), predicting one step is enough. The similar idea also appeared in 12. However, one-step-prediction may make the planner reject disturbances in a quite aggressive way. Herein, to a further degree, we will compare the disturbance rejection performances under one-step-prediction and two-steps-prediction. The second debate is, among relative step location track- ing (velocity tracking) and absolute step location tracking (position tracking), which is better? Although most work focused on position tracking, more researchers began turning to velocity tracking 15, 19, 20, which endows robot with more controlability. However, velocity tracking probably results in large position tracking error, which should be avoided when walking in particular scenarios, such as the narrow space or small contact surface. Herein, the choice of velocity or position reference would be discussed in depth. In this paper, a baseline optimization approach for step location and duration modulation is fi rst proposed, that minimizes the tracking errors of fi nal CoM state of each step. In our previous work 21, the step-duration related variables, namely, the hyperbolic functions of the remain- ing time were introduced to substitute the step time itself 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE2259 x z w c cx(0)cx(T) sx cx(te)cx(te) CoM dx f cz Fig. 1: LIP motion in sagittal plane, cx(0), cx(te)and cx(T)rep- resent the initial, current and fi nal CoM position, respectively, dx represents the global step location, sxis the step length, wand c represents the origin of the global coordinate and local coordinate. as the optimization variables and a NLP was formulated. However, the original NLP was merely seen as a function supplementary and it had to rely on the second-layered NMPC to realize robust walk. In this paper, by considering the feasibility constraints of swing foot velocity and CoM velocity jerk, we extend the previous work and establish a functionally-independent NLP. By using the relative step location as the reference values (that is, in velocity tracking mode), it works as a baseline optimizer. It turned out that the baseline NLP can work effectively for online modulation of step duration and step location. Particularly, as done in 21, the baseline NLP can also be solved fast by Sequential Quadratic Programming (SQP). Then, comparison studies are conducted to demonstrate the versatility of this approach. Different from previous works, the baseline NLP can be switched into the one- step-prediction mode by adding equality constraints of step duration and location of the next step. Besides, by changing the references, the approach can also work in position track- ing mode. Particularly, no matter which mode is imposed, the inner structure of NLP is kept to be the same and the computational cost is suppressed in a very low level. The rest of this paper is organized as follows. In Section II, the preliminaries are briefl y reviewed. Then, in Section III, the NLP formulation for step during and location optimiza- tion is explained. Also, the switch mechanism between one- step-prediction and two-steps-prediction optimization under velocity driven or position driven is discussed in detail. After then, the simulation results are analyzed in Section IV. Finally, the conclusions are drawn in Section V. II. PRELIMINARIES A. LIPM-based Walking Pattern Generation Using the LIPM 22, the CoM movement relative to support center is determined by cx= 2cx, cy= 2cy, = p g/cz,(1) where cx,cy,czTdenote the CoM position relative to the support center, among which czis a constant, g is the grav- itational acceleration and denotes the natural frequency. By solving (1) as an initial value problem, the CoM state (position and velocity) at arbitrary time within current step cycle is determined by (talking the sagittal motion for instance, as can be seen in Fig. 1), cx(te)= cx(0)cosh(te) + cx(0) sinh(te), cx(te)= cx(0) sinh(te) + cx(0)cosh(te), (2) where sinh() and cosh() denote the hyperbolic sine and cosine function, respectively, teis the elapsed time of current step, cx(0), cx(0)Tand cx(te), cx(te)Trepresent the initial and current CoM state, respectively. Based on (2), for a given current CoM state, the fi nal CoM state of each walking cycle is determined by cx(T)= cx(te)cosh(T te) + cx(te) sinh(T te), cx(T)= cx(te) sinh(T te) + cx(te)cosh(T te), (3) where T is step duration, cx(T), cx(T)T is fi nal CoM state. That is, the CoM movement is fully determined by two boundary conditions. Thus, for different task priorities, dif- ferent boundary conditions should be chosen. Herein, in order to track reference step locations and step time, the current CoM position feedback and the fi nal CoM position reference are used as boundary conditions. Namely, given the current CoM state feedback and reference step parameters of the ithwalking cycle, the CoM trajectory is bounded by cx(te)= cfb x(te), cx(iT)= isx/2, (4) where, cfb x(te) is the CoM position feedback, isx is the ithstep length, iT is the ith step duration. Intuitively, isx/2 is set to be the reference fi nal CoM position of the ithstep. Using above formulas, stable walking patterns by tracking the reference step parameters can be generated. B. SQP for NLP solution Given a constrained NLP expressed as min X f(X) s.t.hj(X) 0,j 1,.,Nc, (5) where X Ntis the state vector, Nc, Ntare the number of constraints and state variables, respectively. If the objective function and constraints in (5) are twice continuously differentiable, the problem can be solved by SQP algorithm, via min X 1 2 T X 2 X(f(X)X+(Xf(X) TX, s.t.(Xhj(X)TX+hj(X) 0,j1,.,Nc. (6) With (6), the NLP would be transformed to be the QP, which can be solved fast. The solution of the QP (X 3.(16) The C+ optimization library QuadProg+ is used to solve the QP. Simulation results demonstrated that, although the time cost of each control loop varied depending on the initial status, it was less than 85 s on a 3.0 GHz quad-core CPU, which is much shorter than works in 811. As a result, the proposed NLP can be implemented on a real robot in real time1. V. CONCLUSION In this paper, we proposed a robust walking pattern gen- erator, which can adjust the step duration and step locations 1Actually, the real time whole-body simulations using the COMAN robot have also been conducted, which can be seen in attached video 24. 2263 00.20.40.60.81 x m -0.1 -0.05 0 0.05 0.1 0.15 y m O-V-O 00.20.40.60.81 x m -0.1 -0.05 0 0.05 0.1 0.15 y m T-V-O CoMFoot location 00.20.40.60.81 x m -0.1 -0.05 0 0.05 0.1 0.15 y m T-P-O push pushpush Fig. 6: Horizontal CoM trajectory and step locations generated by using three strategies. ForwardBackwardLeftwardRightward push direction 0 5 10 15 20 25 maximal impulse Ns MPC 1 0-V-0 T-V-0 T-P-0 Fig. 7: Maximal external impulse from which the robot can recover under different approaches, all the pushes were imposed at 2 s. Since the robot is with the right support, the maximal tolerant impulses along the leftward direction are larger than those along the rightward direction. simultaneously. By introducing the time-duration related variables to substitute the hyperbolic functions of the step duration, the constrained nonlinear programming problem can be solved effi ciently via SQP. Apart from the high time effi ciency, different control modes are also realized. Comparison studies demonstrate that our method can realized seamless switch from two-steps- prediction to one-step-prediction and from velocity-tracking to position-tracking. It has been demonstrated that, compared with step location adjustment, step duration optimization can highly enhance the recovery capability from external disturbances. Besides, the strategies under different control modes will show different advantages in different scenarios. In the future, the human-like swing foot trajectory can be realized by using the state-of-the-art imitation learning strategies, such as the works in 25 and 26. Furthermore, event-based switch control can be further studied. ACKNOWLEDGEMENT This work is supported by National Natural Science Foundation of China (Grant No. 51675385) and European Unions Horizon 2020 robotics program CogIMon (ICT-23- 2014, 644727). Besides, the fi rst author is also funded by China Scholarship Council (CSC). APPENDIX A. Explicit Expression of Objective Function To simplify the expression, the variable selection matrices Sj are fi rst defi ned as X(k)=SjX, Sk1Nt, k 1,.,8,Nt=8.(17) Further analysis demonstrates that, most cost terms can be expressed in a quadratic form w.r.t the vector X except the two cost terms about the tracking errors of i+1cx(T) and i+1cy(T). To be brief, only the explicit expressions of some terms in (9) are discussed here as the representatives. For these terms which are independent to current CoM state, such as the tracking error of isx, we have f(X)isx=k isxisref x k2 = X TST 1S1X2( isref x S1)X+isx, (18) where isxrepresents the constant term For the tracking errors of the fi nal CoM position of the next step (taking i+1cx(T) for instance), we have f(X)i + 1cx(T)=k i+1cx(T)i+1cref x(T) k2 =k X TH1Xi+1cref x(T) k2 (19) where, H1=cfb x(te)(S T 3S7+S T 4S8)+ cfb x(te) (ST 4S7+S T 3S8)S T 1S7. Thus, the objective function is the 4thpolynomial w.r.t X. However, using the SQP algorithm, we just care about the fi rst order and second order derivatives. Using the matrix derivative theory, they can be derived easily. Thus, the NLP can be solved fast by SQP. B. Explicit expressions of Feasibility Constraints Briefl y, only the equality constraint on tchand tshand the inequality constraint of the CoM velocity jerk are discussed. During the current step, the tchand tshshould satisfy following equality constraint, it2 ch it2 sh 1 = 0. (20) Then, the explicit expression is heq(X)itchitsh= X T(ST 3S3 S T 4S4)X 1. (21) As for the inequality constraint of CoM velocity jerk, only the upper boundary is discussed here. We have: cx(te+dt) cfb x(te) c max x dt 0.(22) 2264 TABLE III: Algorithm parameters setup for the NLP ic x(T) 1106ic y(T) 1106 is x 1107is y 1107 it ch 5108it sh 5108 i + 1c x(T) 10i + 1c y(T) 10 i + 1s x 100i + 1s y 100 i + 1t ch 1103i + 1t sh 1103 i cx(T)5104i cy(T)5104 dts0.05gms?29.8 mkg31czm0.467 TABLE IV: Parameters for constraints of the NLP Step location constraintsStep duration constraints smin x m-0.05Tmin x s0.4 smax x m0.3Tmax x s1.2 smin y m0.11CoM acceleartion constraints smax y m0.2 cmin x ms?2-2 smin x ms?1-0.9 cmax x ms?22 smax x ms?11 cmin y ms?2-2 smin y ms?1-0.45 cmax y ms?22 smax y ms?10.9/ Considering the boundary conditions listed in (4), the explicit expression can be obtained as hiq(X) cx(te+dt)= (a1S1+ a2S3+ a3S4)X.(23) where, a1= cosh(dt), a2= 2a1cfb x(te), a3= 2(cfb x(te)sinh(dt) c fb x(te) c max x dt). C. Parameters setup for simulation test The parameters for simulation studies are listed in Table III and Table IV. The NLPs under different control authorities used the same parameters setup. REFERENCES 1 H. Diedam, D. Dimitrov, P.-B. Wieber, K. Mombaur, and M. Diehl, “Online walking gait generation with adaptive foot positioning through linear model predictive control,” in IEEE/RSJ International Confer- ence on Intelligent Robots and Systems, 2008, pp. 11211126. 2 A. Herdt, H. Diedam, P.-B. Wieber, D. Dimitrov, K. Mombaur, and M. Diehl, “Online walking motion generation with automatic footstep placement,” Advanced Robotics, vol. 24, no. 5-6, pp. 719737, 2010. 3 R. J. Griffi n and A. Leonessa, “Model predictive control for dynamic footstep adjustment using the divergent component of motion,” in IEEE International Conference on Robotics and Automation, 2016, pp. 17631768. 4 S. Feng, X. Xinjilefu, C. G. Atkeson, and J. Kim, “Robust dynamic walking using online foot step optimization,” in IEEE/RSJ Interna- tional Conference on Intelligent Robots and Systems, 2016, pp. 5373 5378. 5 J. Ding, C. Zhou, S. Xin, X. Xiao, and N. Tsagarakis, “Nonlin- ear model predictive control for robust bipedal locomotion explor- ing com height and angular momentum changes,” arXiv preprint arXiv:1902.06770, 2019. 6 Q. Li, I. Chatzinikolaidis, Y. Yang, S. Vijayakumar, and Z. Li, “Robust foot placement control for dynamic walking using online parameter estimation,” in IEEE-RAS International Conference on Humanoid Robotics, 2017, pp. 165170. 7 J. Englsberger, C. Ott, and A. Albu-Sch affer, “Three-dimensional bipedal walking control based on divergent component of motion,” IEEE Transactions on Robotics, vol. 31, no. 2, pp. 355368, 2015. 8 Z. Aftab, T. Robert, and P.-B. Wieber, “Ankle, hip and stepping strate- gies for humanoid balance recovery with a single model predictive control scheme,” in IEEE-RAS International Conference on Humanoid Robots, 2012, pp. 159164. 9 P. Kryczka, P. Kormushev, N. G. Tsagarakis, and D. G. Caldwell, “Online regeneration of bipedal walking gait pattern optimizing foot- step placement and timing,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 20