Formula understanding in quadruped control

• explain ： This article is used to record the formula understanding of quadruped related papers , Due to my limited ability , The understanding of the formula comes from studying the content of the paper , A combination of online articles and personal guesses , You are welcome to criticize and correct any inaccuracies , This article will also be updated . Welcome to contact me ：[email protected]

《MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot》

1. Support leg controller

（1） Based on the centralized quality model (lumped model) Balance controller for (Balance Controller)

i. The motion equation of the robot which regards the robot as a single rigid body model :
$$\underset{A}{\underbrace{\left[\begin{array}{ccc}{I}_3& \dots & I_3\\ [p_1{p}_c]\times & \dots & [p_4{p}_c]\times \end{array}\right]}}F=\underset{b}{\underbrace{\left[\begin{array}{c}m({\ddot{p}}_c+g)\\ I_G\dot{{\omega }_b}\end{array}\right]}}$$

Using the basic Newton's laws of motion

• Together = ma
• Resultant moment = inertia * Angular acceleration

ii. Use PD The control law calculates the centroid acceleration and angular acceleration
$$\left[\begin{array}{c}{\ddot{\mathbf{p}}}_{c,d}\\ {\dot{\mathbf{\omega }}}_{b,d}\end{array}\right]=\left[\begin{array}{c}{\mathbf{K}}_{p,p}\left({\mathbf{p}}_{c,d}-{\mathbf{p}}_c\right)+{\mathbf{K}}_{d,p}\left({\dot{\mathbf{p}}}_{c,d}-{\dot{\mathbf{p}}}_c\right)\\ {\mathbf{K}}_{p,\omega }\log \left({\mathbf{R}}_d{\mathbf{R}}^T\right)+{\mathbf{K}}_{d,\omega }\left({\mathbf{\omega }}_{b,d}-\mathbf{\omega }\right)\end{array}\right]$$
The goal is ： Get the optimal control force F, Make the center of mass tend to the ideal center of mass dynamic state
$${\mathbf{b}}_{\mathbf{d}}=\left[\begin{array}{c}m({\ddot{\mathbf{p}}}_{c,d}+\mathbf{g})\\ {\mathbf{I}}_{\mathbf{G}}{\dot{\mathbf{\omega }}}_{b,d}\end{array}\right]$$
Because the robot motion equation is linear , The optimization problem can be transformed into QP problem
$$\begin{gathered} {\mathbf{F}}^{\ast }=\underset{F\in R^{12}}{\min }(AF-b_d{)}^{T}S(AF-b_d)+\alpha ||F|{|}^2+\beta ||F-F_{prev}|{|}^2 \\ s.t.CF\le d \end{gathered}$$
significance ：

• $(AF-b_d{)}^{T}S(AF-b_d)$ ---- Make the center of mass state meet the expectation ,S The matrix represents the priority of rotation and displacement in the control .
• $||F|{|}^2$ ---- Minimize the force
• $||F-F_{prev}|{|}^2$ ----$F_{prev}$ Represents the last force in the iteration , The goal is to limit the difference between the solutions of the two iterations .
• $\alpha ,\beta >0$ ---- Specify the standardization of knowledge , And filter the solution to some extent .
• $CF\le d$ ---- The details are unknown , The purpose is to ensure that the solution is in the feasible region .

（2）MPC controller (Model Predictive Control)

$$J=\sum _{i=0}^k(x_i-x_i^{ref}{)}^T{Q}_i(x_i-x_i^{ref})+u_i^T{R}_i{u}_i$$
The goal is ： Constantly updated $u_i$ Input to the control system , Minimize the above cost function .

• The update frequency f： In this paper, the optimal solution is given by 30Hz The frequency of is input to the controller
• Solving process ： Paper points out , Need to put MPC Problem into QP Solve the problem .
  MPC For details of the controller, please refer to my study notes ：MPC Learning notes

2. Swing leg controller

（1） Foot drop point calculation

$$p_{step,i}=p_{h,i}+\underset{RaibertHeuristic}{\underbrace{\frac{T_{c_{\varphi }}}{2}{\dot{p}}_{c,d}}}+\underset{CapturePoint}{\underbrace{\sqrt{\frac{z_0}{||g||}}({\dot{p}}_c-{\dot{p}}_{c,d})}}$$
$\frac{T_{c_{\varphi }}}{2}{\dot{p}}_{c,d}$ ---- according to 《Legged robots that balance》 Description in , The landing point of the foot end determined according to the formula , Will make the motion symmetrical , Acceleration is 0

$\sqrt{\frac{z_0}{||g||}}({\dot{p}}_c-{\dot{p}}_{c,d})$ ---- according to 《Capture point: A step toward humanoid push recovery》 Description in , The goal is to make the foothold closer capture point, At this point the legs will be balanced .

$T_{c_{\phi }}$ ---- In an ideal state stance State duration
$z_0$ ---- The height of the target location point
$p_{h,i}$---- leg i Corresponding hip motor hip The location of

（2） Swing trajectory calculation –PD controller + Feedforward control

i. Calculation of feedforward torque
$${\tau }_{\mathrm{f}\mathrm{f},i}={\mathbf{J}}_i^{\top }{\mathbf{\Lambda }}_i\left({}^{\mathfrak{B}}{\mathbf{a}}_{i,\text{ ref }}-{\dot{\mathbf{J}}}_i{\dot{\mathbf{q}}}_i\right)+{\mathbf{C}}_i{\dot{\mathbf{q}}}_i+{\mathbf{G}}_i$$
$J_i$ ---- Jacobian matrix of the foot
${\Lambda }_i$ ---- Inertia matrix of operation space
${}^{\mathfrak{B}}{a}_{i,ref}$ ---- Reference acceleration
$q_i$ ---- Vector of joint structure
$C_i$ ---- Coriolis matrix
$G_i$ ----- Heavy moment

ii. Track following controller
$${\mathbf{\tau }}_i={\mathbf{J}}_i^{\top }\left[{\mathbf{K}}_p\left({}^{\mathfrak{B}}{\mathbf{p}}_{i,\text{ ref }}-{}^{\mathfrak{B}}{\mathbf{p}}_i\right)+{\mathbf{K}}_d\left({}^{\mathfrak{B}}{\mathbf{v}}_{i,\mathrm{r}\mathrm{e}\mathrm{f}}-{}^{\mathfrak{B}}{\mathbf{v}}_i\right)\right]+{\mathbf{\tau }}_{\mathrm{f}\mathrm{f},i}$$

• Control frequency f：4.5kHz

In order to make PD The controller remains stable , You need to adjust the gain $K_p$ Do the following
$$K_{p,j}={\omega }_{des}^2{\Lambda }_{jj}(q)$$

• $K_{p,j}$ ---- $K_p$ No j Diagonal terms .
• ${\omega }_{des}$ ---- Target natural frequency
• ${\Lambda }_{jj}$ ---- The first j Inertia matrix of diagonal terms in operation space

3. Virtual Prediction supports polygons （Virtual Predictive Support Polygon）

i. Calculate the weight factor for each leg

（1） Virtual support polygon ： Provide centroid position points under various gait
（2） Define the contribution of each leg to the prediction polygon （ Phase by phase ）： Point out which leg should be raised or touched , And the time of state switching
$$\begin{array}{r}{K}_{c_{\varphi }}=\frac{1}{2}\left[\mathrm{erf}\left(\frac{\varphi }{{\sigma }_{c_0}\sqrt{2}}\right)+\mathrm{erf}\left(\frac{1-\varphi }{{\sigma }_{c_1}\sqrt{2}}\right)\right]\\ \\ K_{{\bar{c}}_{\varphi }}=\frac{1}{2}\left[2+\mathrm{erf}\left(\frac{-\varphi }{{\sigma }_{{\bar{c}}_0}\sqrt{2}}\right)+\mathrm{erf}\left(\frac{\varphi -1}{{\sigma }_{{\bar{c}}_1}\sqrt{2}}\right)\right]\\ \\ \Phi =s_{\varphi }{K}_{c_{\varphi }}+{\bar{s}}_{\varphi }{K}_{{\bar{c}}_{\varphi }}\end{array}$$
$erf(x)$ ---- Gaussian error function
$K_{c_{\varphi }},K_{{\bar{c}}_{\varphi }}$ ---- Weight factor of support phase and swing phase
$\Phi$ ---- The total weight factor of the leg
meaning ： In the supported state , The closer a leg is to the center , The more it can be used as a support point , Under the dynamic pendulum , The closer a leg is to the center , The less it can be used to balance

ii. Calculate the virtual points of each leg

$$\left[\begin{array}{c}{\mathbf{\xi }}_i^{-}\\ {\mathbf{\xi }}_i^{+}\end{array}\right]=\left[\begin{array}{ll}{\mathbf{p}}_i& {\mathbf{p}}_{i^{-}}\\ {\mathbf{p}}_i& {\mathbf{p}}_{i^{+}}\end{array}\right]\left[\begin{array}{c}{\Phi }_i\\ 1-{\Phi }_i\end{array}\right]$$

• A positive superscript indicates counterclockwise , A negative superscript indicates clockwise

iii. Calculate the predicted polygon vertices for each leg

$${\mathbf{\xi }}_i=\frac{{\Phi }_i{\mathbf{p}}_i+{\Phi }_i{\mathbf{\xi }}_i^{-}+{\Phi }_{i+}{\mathbf{\xi }}_i^{+}}{{\Phi }_i+{\Phi }_{i^{-}}+{\Phi }_{i^{+}}}$$

iv. Calculate the desired centroid position

$${\hat{\mathbf{p}}}_{CoM,d}=\frac{1}{N}\sum _{i=1}^N{\mathbf{\xi }}_i$$

4. Attitude adjustment in sloping terrain

meaning ： Represents a plane , That is, the plane where the quadruped robot is currently located
$$z(x,y)=a_0+a_{1}x+a_{2}y$$
meaning ： Get a plane according to the current state of the robot , namely a={ $a_0,a_1,a_2$}
$$\begin{array}{rl}\mathbf{a}& ={\left({\mathbf{W}}^T\mathbf{W}\right)}^{†}{\mathbf{W}}^T{\mathbf{p}}^z\\ \mathbf{W}& ={\left[\begin{array}{lll}1& {\mathbf{p}}^x& {\mathbf{p}}^y\end{array}\right]}_{4\times 3}\end{array}$$

• ${\mathbf{p}}^{\mathbf{x}},{\mathbf{p}}^{\mathbf{y}},{\mathbf{p}}^{\mathbf{z}}$ ---- Including the status of each leg of the robot , such as ${\mathbf{p}}^{\mathbf{x}}=(p_1^x,p_2^x,p_3^x,p_4^x)$

5. State estimation

It uses Kalman filtering and Extended Kalman filter , I won't elaborate here .

---

《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》

1. Hybrid controller

The goal is ： Split the position control into two simple controllers to control

（1）MPC controller

$$\begin{array}{l}m\ddot{\mathbf{p}}={\sum }_{i=1}^{n_c}{\mathbf{f}}_i-{\mathbf{c}}_g,\\ \\ \frac{d}{dt}(\mathbf{I}\mathbf{\omega })={\sum }_{i=1}^{n_c}{\mathbf{r}}_i\times {\mathbf{f}}_i\end{array}$$

• Similar to the above balance controller , Objects are Lumped mass model , But here we use MPC To find the optimal control force .

（2）WBIC controller （Whole-Body Impulse Control）

$$\mathbf{A}\left(\begin{array}{c}{\ddot{\mathbf{q}}}_f\\ {\ddot{\mathbf{q}}}_j\end{array}\right)+\mathbf{b}+\mathbf{g}=\left(\begin{array}{c}{0}_6\\ \mathbf{\tau }\end{array}\right)+{\mathbf{J}}_c^{\top }{\mathbf{f}}_r$$
$A$ ---- quality ( inertia ) matrix
$b$ ---- Coriolis force
$g$ ---- gravity
$\tau$ ---- Joint torque
$f_r$ ---- Increased force
$J_c$ ---- Jacobian matrix of touchdown point

Understanding of iterative formulas

Reference resources Hand push WBC The formula

Continuous updating …