## Introduction

One good way of generating a centre of mass trajectory is the quadratic programming method proposed in [1].

The idea pioneered in [2] is to generate a centre of mass trajectory based on a zero moment point trajectory, in turn the zero moment point trajectory can be generated by deciding the foot placement positions(as shown in the figure to the right, sometimes theres little choice in feasible footstep positions) and making a ZMP trajectory that is always inside the support region. Centre of the support foot during single support phase and a 5th order polynomial between the two feet during double support phase is one option(see the appendix at the bottom for more on polynomial trajectories).

The idea in [1] was to do the same thing as [2] but via quadratic programming. This has some advantages such as allowing for constraints on the ZMP position and, in my experience, being easier to implement.

First I am going to explain how trajectories can be generate using quadratic programing in general and then I will explain the specifics of how to it can be used to generate a COM trajectory.

Given a linear system
$latex X_{k+1}=AX_{k}+Bu_{k}$
$latex z_{k}=CX_{k}$

Here X is a 3×1 control variable as follows
$latex x=\left[\begin{array}{c} x\\ \dot{x}\\ \ddot{x} \end{array}\right]$
and u is the jerk i.e $latex u=\dddot{x}$
N values of the systems output can be described by the following linear system
$latex \bar{Z}=\bar{M}X_{0}+\bar{N}\bar{u}$

where $latex X_{0}$ is the initial state of the system, \bar{M} and \bar{N} are as follows.
$latex \bar{M}=\left[\begin{array}{c} CA\\ CA^{2}\\ \vdots\\ CA^{N} \end{array}\right]$
$latex \bar{N}=\left[\begin{array}{cccc} C & 0 & \ldots & 0\\ CAB & CB & & 0\\ \vdots & & \ddots & \vdots\\ CA^{N-1}B & CA^{N-2}B & \ldots & B \end{array}\right]$

The objective is to make the systems outputs track a certain goal while minimizing actuator effort. This objective can be described mathematically as follows
$latex J=\frac{1}{2}Q(\bar{Z}-\bar{Z}^{ref})^{2}+\frac{1}{2}R\bar{\dddot{x}} \bar{Z}=\left[\begin{array}{c} z_{0}\\ z_{1}\\ \vdots\\ z_{N} \end{array}\right]\\ \bar{Z}^{ref}=\left[\begin{array}{c} z_{0}^{ref}\\ z_{1}^{ref}\\ \vdots\\ z_{N}^{ref} \end{array}\right] \bar{X}=\left[\begin{array}{c} X_{0}\\ X_{1}\\ \vdots\\ x_{N} \end{array}\right]$

Here Q and R are the performance and cost indexes respectively and $latex z_{k}^{ref}$ represents the desired output state at time k. The above can be solved as a quadratic program by rearranging it into the form shown below

$latex \underset{\bar{\dddot{x}}}{min}\quad f(\bar{\dddot{x}})=\frac{1}{2}\bar{\dddot{x}}^{T}w\bar{\dddot{x}}+v\bar{\dddot{x}}$
Here w and v are as shown below
$latex w=QN^{T}N+RI_{N\times N} v=QN^{T}Mx_{0}-QN^{T}\bar{Z}^{ref}$

The above quadratic program can be solved simply as follows
$latex \dddot{x}=-w^{-1}v$

Now that the general method of generating trajectories using quadratic programming has been discussed, ZMP preview control can be realized simply by using the following A,B and C matrices
$latex A=\left[\begin{array}{ccc} 1 & T & \frac{T^{2}}{2}\\ 0 & 1 & T\\ 0 & 0 & 1 \end{array}\right]\label{eq:AMat}$

$latex B=\left[\begin{array}{c} \frac{T^{3}}{6}\\ \frac{T^{2}}{2}\\ T \end{array}\right]$
$latex C=\left[\begin{array}{ccc} 1 & 0 & -h_{com}/g\end{array}\right]$
Here $latex h_{com}$ is the center of mass height, g is the gravity constant, T is the sampling period. The A and B matrices implement a integrator system and the C matrix implements a ZMP output.

A step ZMP input should provide the following COM trajectory response. Note how the COM starts moving before the ZMP reference value changes so as to minimize global ZMP error.

## References

[1] “Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations” Pierre-Brice Wieber
[2] “Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point” Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kiyoshi Fujiwara, Kensuke Harada, Kazuhito Yokoi and Hirohisa Hirukawa, Proceedings of the 2003 IEEE International Conference on Robotics & Automation, Taiwan, 2003

## Appendix

A 5th order polynomial is a useful tool for defining trajectories as a function of the start and end positions, velocities and accelerations. It is defined as follows.
$latex x(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$
where the coefficients are calculated as follows

$latex \begin{array}{c} a_{0}=x_{start}\\ a_{1}=\dot{x}_{start}\\ a_{2}=\frac{\ddot{x}_{start}}{2}\\ a_{3}=\frac{20x_{end}-20x_{start}-(8\dot{x}_{end}+12\dot{x}_{start})t_{end}-(3\ddot{x}_{start}-\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{3}}\\ a_{4}=\frac{30x_{start}-30x_{end}+(14\dot{x}_{end}+16\dot{x}_{start})t_{end}+(3\ddot{x}_{start}-2\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{4}}\\ a_{5}=\frac{12x_{end}-12x_{start}-(6\dot{x}_{end}+6\dot{x}_{start})t_{end}-(\ddot{x}_{start}-\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{5}}\end{array}$

https://i1.wp.com/nvtienanh.com/wp-content/uploads/2016/01/preview.png?fit=640%2C329https://i1.wp.com/nvtienanh.com/wp-content/uploads/2016/01/preview.png?resize=150%2C150Mechatronicsbiped robot,ZMP TrajectoryIntroduction One good way of generating a centre of mass trajectory is the quadratic programming method proposed in . The idea pioneered in is to generate a centre of mass trajectory based on a zero moment point trajectory, in turn the zero moment point trajectory can be generated by deciding...