Open MiladShafiee opened 5 years ago
MiladShafiee
commented
5 years ago In this section our goal is to control the $CMP$($ZMP$ and $\dot{H}$) to compensate the error of DCM. \begin{equation} \gamma_T+r^{zmp}_T+ \left(r^{cmp}2- \xi{curr} -\frac{\delta}{2} \right)\sigma=r^{cmp}_1 +\frac{\delta^{cmp}}{2}. \end{equation}
In this way first of all we define the following terms:
MiladShafiee
commented
5 years ago In this section our goal is to control the $CMP$($ZMP$ and $\dot{H}$) to compensate the error of $DCM$. \begin{equation} \gamma_T+r^{zmp}_T+ \left(r^{cmp}2- {\xi}{curr} -\frac{\delta}{2} \right)\sigma=r^{cmp}_1 +\frac{\delta^{cmp}}{2}. \end{equation}
In this way first of all by considering a reference value for terms of above equation and error with respect these reference values, we define the following terms:
\begin{equation} \xi{curr} =\xi{curr,ref} +\xi{curr,error} \end{equation}
\begin{equation} \gammaT=\gamma{T,ref}+\gamma_{T,err} \end{equation}
\begin{equation} r^{cmp,1}=r^{cmp1,ref}+r^{cmp1,cntrl} \end{equation}
It's noteworthy that $\gamma{T,err}=\xi{T,err}-r^{zmp,T}$ Also just for problem of loading latex command in above equation I wrote $r^{cmp1,ref}=r^{cmp}_{1,ref}$ and $r^{zmp}_T=r^{zmp,T}$.
By substituting the eqqqqq 333 to the eq111 we will have: \begin{equation} \gamma_T+r^{zmp}_T+ \left(r^{cmp}2- \xi{curr} -\frac{\delta}{2} \right)\sigma=r^{cmp}_1 +\frac{\delta^{cmp}}{2}. \end{equation}
Push Recovery by Ankle, Hip, Step timing and Position Adaptation Strategies by a Single QP Problem
Notation:
$T$ : Step timing(single support duration)
$r^{zmp}_1 $ , $r^{zmp}_2 \in \mathbb{R}^2$ : First and Second ZMP position during single support(swing mode).
$r^{cmp}_1 $ , $r^{cmp}_2 \in \mathbb{R}^2$ : First and Second CMP position during single support(swing mode).
$r^{zmp}_T \in \mathbb{R}^2$ : The next step position. (The landing position of swing foot)
$\delta^{cmp}=r^{cmp}_2-r^{cmp}_1 \in \mathbb{R}^2$
$\xi_0 \in \mathbb{R}^2$ : Initial DCM position
$\xi_T \in \mathbb{R}^2$ : Final DCM position
$\gamma_T$ =$\xi_T-r^{zmp}_T$ : DCM offset
$\sigma \in \mathbb{R}$ = $e^{\frac{T}{b}}$
$H_y \in \mathbb{R}$ : Angular momentum around CoM in y coordinate
We employ a single QP problem which uses a combination of Center of Pressure (CoP) manipulation, step position and timing adjustment, and Centroidal Moment Pivot (CMP) modulation to design a robust walking controller. Furthermore, we exploit the concept of time-varying ZMP during the single support to generalize our walking controller. In our previous ISUUUESSSSS we have found the following equation for step timing and position adjustment.
\begin{equation} \gamma_T+r^{zmp}_T+ \left(r^{zmp}_2- \xi_0 -\frac{\delta}{2} \right)\sigma=r^{zmp}_1 +\frac{\delta^{zmp}}{2}. \end{equation}
By adding a flywheel to the LIPM model as an linear approximation of the Centroidal angular momentum, the Centroidal Momentum Pivot(CMP) will be defined:
\begin{equation} {r}^{cmp}={r}^{zmp}+\frac{1}{ m{g}}\big[\dot{H}_y,-\dot{H}_x\big]^T \end{equation}
The${r}^{cmp}$ is equal to the ${r}^{zmp}$ in the case of zero moment around the CoM. For a non-zero moment about the CoM, however, the CMP can move beyond the edges of the support polygon, while the ZMP still remains inside the support polygon. In other words, the CMP is the point where a line parallel to the ground reaction force and passing through the CoM intersects the ground.
So eqqqq1 will be modified by considering the CMP as an input. Therefore the preplanned CMP trajectory will be as following:
\begin{equation} r^{cmp}(t)=A e^{-\frac{t}{b }} + B \quad , r^{cmp}(0) =r^{cmp}_1 \quad , \quad r^{cmp}(T) =r^{cmp}_2 \end{equation} It's noteworthy that by planning CMP trajectory, actually we are planning the both derivative of the centroidal angular momentum and ZMP by setting the initial condition for example like this:
\begin{equation} \begin{split} {r}^{cmp}_1={r}^{zmp}_1+\frac{1}{ m{g}}\big[\dot{H_1}_y,-\dot{H_1}_x\big]^T
\end{split} \end{equation}
So the eqqqqq will be found as following:
\begin{equation} \gamma_T+r^{zmp}_T+ \left(r^{cmp}_2- \xi_0 -\frac{\delta}{2} \right)\sigma=r^{cmp}_1 +\frac{\delta^{cmp}}{2}. \end{equation}
So up to here we can use the pre-planned CMP(centroidal angular momentum and ZMP) during step adjustment.
Assumptions: This controller just talks about swing phase(single support).
Our Contribution: Unlike previous researches about DCM based step position and timing adadptation, (Hof, Khadiv), that considered the constant CoP position during swing phase, our proposed algorithm allow us to plan the CoP trajectory during swing phase.
Note: All defined parameter are scalars, and can be used for sagittal or lateral direction.
Notice: In order to eliminate the nonlinear view of step timing variable, we will use the following two change of variable for the step duration(sometimes we use first one, sometime the second one based on the view of equations!): \begin{equation} \gamma = e^{-\omega . T} \quad , \quad \sigma = e^{\omega . T} \end{equation}
First step: We consider that CoP will move from $p_1$ to $p_2$ during SS phase. Therefore, we consider the following interpolation for CoP trajectory: \begin{equation} p(t)=A . e^{-\omega . t} + B \quad , p(0) =p_1 \quad , \quad p(T) =p_2 \label{eq:2} \end{equation} By using these final and initial condition, we will find A and B as follows: \begin{equation} A =\frac{p_2-p_1}{\gamma - 1} \quad , \quad B =\frac{p_1 .\gamma - p_2}{\gamma - 1} \end{equation}
Second Step : For the DCM dynamics we have a linear first order inhomogeneous ode: \begin{equation} \dot {\xi}=\omega (\xi-p(t))
\end{equation} The solution of linear first order inhomogeneous ode in the form $\dot{x}+R(t) x=Q(t) $ will be as follows: \begin{equation} x(t)=e^{- \int R(t) dt} . \left( \int Q(t) . e^{\int R(t) dt} + C \right) \end{equation} By writing the DCM equation (\ref{eq:4}) in the above format, we will have :
\begin{equation} \dot {\xi} - \omega \xi=-\omega . p(t)=-A . \omega . e^{-\omega . t} - B. \omega \label{eq:6}
\end{equation} Therefore the solution of equation (\ref{eq:6}) can be expressed by the following integral: \begin{equation} \xi(t)=e^{- \int -\omega dt} . \left( \int (-A . \omega . e^{-\omega . t} - B. \omega) . e^{\int -\omega dt} + C \right) \end{equation}
and by solving the integral of (\ref{eq:7}), we will have: \begin{equation} \xi (t)=\frac{A}{2} . e^{-\omega . t} + B + C . e^{\omega . t} \end{equation}
In the (\ref{eq:8}) $C$ is unknown. We can find it by solving above problem as a initial value problem or as a final value problem!
By solving (\ref{eq:8}) as a initial value problem, we will have: \begin{equation} \xi(0)=\xi_0=\frac{A}{2}+B+C \quad \Rightarrow \quad C_0=\xi_0-\frac{A}{2}-B \label{eq:9} \end{equation} By solving (\ref{eq:8}) as a final value problem, we will have: \begin{equation} \xi(T)=\xi_T=\frac{A}{2} . e^{-\omega . T} +B+C . e^{\omega . T} \quad \Rightarrow \quad C_f=(\xi_T-\frac{A}{2} . e^{-\omega . T} -B) . e^{-\omega . T} \end{equation}
For having a unique equation for DCM trajectory that includes both the initial and final problems we consider following condition: \begin{equation} C_0=C_f \end{equation}
Therefore, we will have: \begin{equation} \xi_0-\frac{A}{2}-B=(\xi_T-\frac{A}{2} . e^{-\omega . T} -B) . e^{-\omega . T} \end{equation}
And by using $\gamma = e^{-\omega . T} $ we have : \begin{equation} \xi_0-\frac{A}{2}-B=\xi_T. \gamma-\frac{A}{2} \gamma^2 - B. \gamma \end{equation}
By ordering the equation (\ref{eq:13}) we have: \begin{equation} \xi_T . \gamma - \xi_0=\frac{A}{2}({\gamma^2}-1)+B.(\gamma-1) \end{equation}
By substituting the $A =\frac{p_2-p_1}{\gamma - 1} $ and $B =\frac{p_1 .\gamma - p_2}{\gamma - 1}$ amounts to equation (\ref{eq:14}), we will have:
\begin{equation} \xi_T . \gamma - \xi_0=\frac{p_2-p_1}{2. (\gamma - 1)}({\gamma^2}-1)+\frac{p_1 .\gamma - p_2}{\gamma - 1}.(\gamma-1) \end{equation} By simplifying the equation (\ref{eq:15}), we will have: \begin{equation} \xi_T . \gamma - \xi_0=\frac{p_2-p_1}{2}({\gamma}+1)+{p_1 .\gamma - p_2} \end{equation}
By substituting $\gamma=\frac{1}{\sigma}$ and $\delta=p_2 - p_1$ we will have: \begin{equation} \xi_T - \xi_0 . \sigma=\frac{\delta}{2}(1+{\sigma})+{p_1 - p_2 . \sigma} \end{equation} By adding and subtracting the $p_T$ into the equation (\ref{eq:17}) and defining the DCM offset $b_T=\xi_T-p_T$ we will have: \begin{equation} b_T+p_T-\frac{\delta}{2}(1+{\sigma})-p_1 + p_2 . \sigma- \xi_0 . \sigma=0 \end{equation} Therefore we can use equation (\ref{eq:18}) in an optimization problem to find an step timing and step position adaptation during swing phase. Please note that we can use this equation for the sagittal and lateral direction independently.
Third step: In this part we want to pay attention to the optimization problem corresponding to (\ref{eq:18}). By consider some nominal value for step length and step duration, we will have the following cost function:
\begin{equation} \begin{aligned} J = \alpha1 \left\Vert\left( p{T,x} - L_{nom} \right) \right\Vert^2 + \alpha2 \left\Vert\left( b{T} - b_{nom} \right) \right\Vert^2 + \alpha3 \left\Vert\left( \sigma - \sigma{nom} \right) \right\Vert^2 \end{aligned} \end{equation}
And we will have followoing constraints based on the maximum step length and step duration and also the equation (\ref{eq:18}):
\begin{equation} \begin{bmatrix} 1 & 0 &0 \newline -1 & 0 & 0 \newline 0 & 1 & 0 \newline 0 & -1 & 0
\end{bmatrix} \begin{bmatrix} p{T,x} \newline \sigma \newline b{T,x}
\end{bmatrix} \le \begin{bmatrix} L{max} \newline -L{min} \newline e^{\omega . T{max}} \newline -e^{\omega . T{min}} \end{bmatrix} \end{equation}
\begin{equation} \begin{bmatrix} 1 & p_2-\xi0-\frac{\delta}{2} & 1 \newline \end{bmatrix} \begin{bmatrix} p{T,x} \newline \sigma \newline b_{T,x}
\end{bmatrix} =\frac{\delta}{2}+p_1 \end{equation}
The above optimization problem can be expressed in QP form. At each control cycle the step position and step duration will be find based on the current DCM initial position.